Index: code/branches/physics/src/bullet/AUTHORS
===================================================================
--- code/branches/physics/src/bullet/AUTHORS	(revision 1963)
+++ code/branches/physics/src/bullet/AUTHORS	(revision 1963)
@@ -0,0 +1,22 @@
+
+Bullet Physics Library is an open source project with help from the community at the Physics Forum
+See the forum at http://bulletphysics.com
+
+The project was started by Erwin Coumans
+
+Following people contributed to Bullet
+(random order, please let us know on the forum if your name should be in this list)
+
+Gino van den Bergen: LinearMath classes
+Christer Ericson: parts of the voronoi simplex solver
+Simon Hobbs: 3d axis sweep and prune, Extras/SATCollision, separating axis theorem + SIMD code
+Dirk Gregorius: generic D6 constraint
+Erin Catto: accumulated impulse in sequential impulse
+Nathanael Presson: EPA penetration depth calculation
+Francisco Leon: GIMPACT Concave Concave collision
+Joerg Henrichs: make buildsystem (work in progress)
+Eric Sunshine: jam + msvcgen buildsystem
+Steve Baker: GPU physics and general implementation improvements
+Jay Lee: Double precision support
+KleMiX, aka Vsevolod Klementjev, managed version, rewritten in C# for XNA
+Erwin Coumans: most other source code
Index: code/branches/physics/src/bullet/Bullet-C-Api.h
===================================================================
--- code/branches/physics/src/bullet/Bullet-C-Api.h	(revision 1963)
+++ code/branches/physics/src/bullet/Bullet-C-Api.h	(revision 1963)
@@ -0,0 +1,175 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
+	Work in progress, functionality will be added on demand.
+
+	If possible, use the richer Bullet C++ API, by including "btBulletDynamicsCommon.h"
+*/
+
+#ifndef BULLET_C_API_H
+#define BULLET_C_API_H
+
+#define PL_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifdef BT_USE_DOUBLE_PRECISION
+typedef double	plReal;
+#else
+typedef float	plReal;
+#endif
+
+typedef plReal	plVector3[3];
+typedef plReal	plQuaternion[4];
+
+#ifdef __cplusplus
+extern "C" { 
+#endif
+
+/*	Particular physics SDK */
+	PL_DECLARE_HANDLE(plPhysicsSdkHandle);
+
+/* 	Dynamics world, belonging to some physics SDK */
+	PL_DECLARE_HANDLE(plDynamicsWorldHandle);
+
+/* Rigid Body that can be part of a Dynamics World */	
+	PL_DECLARE_HANDLE(plRigidBodyHandle);
+
+/* 	Collision Shape/Geometry, property of a Rigid Body */
+	PL_DECLARE_HANDLE(plCollisionShapeHandle);
+
+/* Constraint for Rigid Bodies */
+	PL_DECLARE_HANDLE(plConstraintHandle);
+
+/* Triangle Mesh interface */
+	PL_DECLARE_HANDLE(plMeshInterfaceHandle);
+
+/* Broadphase Scene/Proxy Handles */
+	PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
+	PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
+	PL_DECLARE_HANDLE(plCollisionWorldHandle);
+
+/*
+	Create and Delete a Physics SDK	
+*/
+
+	extern	plPhysicsSdkHandle	plNewBulletSdk(); //this could be also another sdk, like ODE, PhysX etc.
+	extern	void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk);
+
+/* Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
+
+	typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);
+
+	extern plCollisionBroadphaseHandle	plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback);
+
+	extern void	plDestroyBroadphase(plCollisionBroadphaseHandle bp);
+
+	extern 	plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+
+	extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle);
+
+	extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+
+/* todo: add pair cache support with queries like add/remove/find pair */
+	
+	extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk);
+
+/* todo: add/remove objects */
+	
+
+/* Dynamics World */
+
+	extern  plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk);
+
+	extern  void           plDeleteDynamicsWorld(plDynamicsWorldHandle world);
+
+	extern	void	plStepSimulation(plDynamicsWorldHandle,	plReal	timeStep);
+
+	extern  void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+
+	extern  void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+
+
+/* Rigid Body  */
+
+	extern  plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape );
+
+	extern  void plDeleteRigidBody(plRigidBodyHandle body);
+
+
+/* Collision Shape definition */
+
+	extern  plCollisionShapeHandle plNewSphereShape(plReal radius);
+	extern  plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z);
+	extern  plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height);	
+	extern  plCollisionShapeHandle plNewConeShape(plReal radius, plReal height);
+	extern  plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height);
+	extern	plCollisionShapeHandle plNewCompoundShape();
+	extern	void	plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn);
+
+	extern  void plDeleteShape(plCollisionShapeHandle shape);
+
+	/* Convex Meshes */
+	extern  plCollisionShapeHandle plNewConvexHullShape();
+	extern  void		plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z);
+/* Concave static triangle meshes */
+	extern  plMeshInterfaceHandle		   plNewMeshInterface();
+	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+
+	extern  void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling);
+
+/* SOLID has Response Callback/Table/Management */
+/* PhysX has Triggers, User Callbacks and filtering */
+/* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */
+
+/*	typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle	rbHandle, plVector3 pos); */
+/*	typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle	rbHandle, plQuaternion orientation); */
+
+	/* get world transform */
+	extern void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
+	extern void	plGetPosition(plRigidBodyHandle object,plVector3 position);
+	extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation);
+
+	/* set world transform (position/orientation) */
+	extern  void plSetPosition(plRigidBodyHandle object, const plVector3 position);
+	extern  void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
+	extern	void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
+
+	typedef struct plRayCastResult {
+		plRigidBodyHandle		m_body;  
+		plCollisionShapeHandle	m_shape; 		
+		plVector3				m_positionWorld; 		
+		plVector3				m_normalWorld;
+	} plRayCastResult;
+
+	extern  int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res);
+
+	/* Sweep API */
+
+	/* extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */
+
+	/* Continuous Collision Detection API */
+	
+	// needed for source/blender/blenkernel/intern/collision.c
+	double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif //BULLET_C_API_H
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp	(revision 1963)
@@ -0,0 +1,38 @@
+
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+
+//
+// btAxisSweep3
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 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.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#include "btAxisSweep3.h"
+
+#include <assert.h>
+
+btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
+{
+	// 1 handle is reserved as sentinel
+	btAssert(maxHandles > 1 && maxHandles < 32767);
+
+}
+
+
+bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
+:btAxisSweep3Internal<unsigned int>(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache,disableRaycastAccelerator)
+{
+	// 1 handle is reserved as sentinel
+	btAssert(maxHandles > 1 && maxHandles < 2147483647);
+}
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h	(revision 1963)
@@ -0,0 +1,1003 @@
+//Bullet Continuous Collision Detection and Physics Library
+//Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+//
+// btAxisSweep3.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 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.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifndef AXIS_SWEEP_3_H
+#define AXIS_SWEEP_3_H
+
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btVector3.h"
+#include "btOverlappingPairCache.h"
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.h"
+#include "btDbvtBroadphase.h"
+
+//#define DEBUG_BROADPHASE 1
+#define USE_OVERLAP_TEST_ON_REMOVES 1
+
+/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
+/// It uses quantized integers to represent the begin and end points for each of the 3 axis.
+/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
+template <typename BP_FP_INT_TYPE>
+class btAxisSweep3Internal : public btBroadphaseInterface
+{
+protected:
+
+	BP_FP_INT_TYPE	m_bpHandleMask;
+	BP_FP_INT_TYPE	m_handleSentinel;
+
+public:
+	
+
+	class Edge
+	{
+	public:
+		BP_FP_INT_TYPE m_pos;			// low bit is min/max
+		BP_FP_INT_TYPE m_handle;
+
+		BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);}
+	};
+
+public:
+	class	Handle : public btBroadphaseProxy
+	{
+	public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+		// indexes into the edge arrays
+		BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3];		// 6 * 2 = 12
+//		BP_FP_INT_TYPE m_uniqueId;
+		btBroadphaseProxy*	m_dbvtProxy;//for faster raycast
+		//void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
+	
+		SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
+		SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
+	};		// 24 bytes + 24 for Edge structures = 44 bytes total per entry
+
+	
+protected:
+	btPoint3 m_worldAabbMin;						// overall system bounds
+	btPoint3 m_worldAabbMax;						// overall system bounds
+
+	btVector3 m_quantize;						// scaling factor for quantization
+
+	BP_FP_INT_TYPE m_numHandles;						// number of active handles
+	BP_FP_INT_TYPE m_maxHandles;						// max number of handles
+	Handle* m_pHandles;						// handles pool
+	
+	BP_FP_INT_TYPE m_firstFreeHandle;		// free handles list
+
+	Edge* m_pEdges[3];						// edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
+	void* m_pEdgesRawPtr[3];
+
+	btOverlappingPairCache* m_pairCache;
+
+	///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+	btOverlappingPairCallback* m_userPairCallback;
+	
+	bool	m_ownsPairCache;
+
+	int	m_invalidPair;
+
+	///additional dynamic aabb structure, used to accelerate ray cast queries.
+	///can be disabled using a optional argument in the constructor
+	btDbvtBroadphase*	m_raycastAccelerator;
+
+
+	// allocation/deallocation
+	BP_FP_INT_TYPE allocHandle();
+	void freeHandle(BP_FP_INT_TYPE handle);
+	
+
+	bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1);
+
+#ifdef DEBUG_BROADPHASE
+	void debugPrintAxis(int axis,bool checkCardinality=true);
+#endif //DEBUG_BROADPHASE
+
+	//Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+	//void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
+
+	
+
+	void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+	void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
+
+public:
+
+	btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
+
+	virtual	~btAxisSweep3Internal();
+
+	BP_FP_INT_TYPE getNumHandles() const
+	{
+		return m_numHandles;
+	}
+
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
+	
+	BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
+	void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher);
+	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+
+	void	processAllOverlappingPairs(btOverlapCallback* callback);
+
+	//Broadphase Interface
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	virtual void  getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
+	void quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const;
+	///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
+	void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_pairCache;
+	}
+	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_pairCache;
+	}
+
+	void	setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback)
+	{
+		m_userPairCallback = pairCallback;
+	}
+	const btOverlappingPairCallback*	getOverlappingPairUserCallback() const
+	{
+		return m_userPairCallback;
+	}
+
+	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+	///will add some transform later
+	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		aabbMin = m_worldAabbMin;
+		aabbMax = m_worldAabbMax;
+	}
+
+	virtual void	printStats()
+	{
+/*		printf("btAxisSweep3.h\n");
+		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+		printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
+			m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
+			*/
+
+	}
+
+};
+
+////////////////////////////////////////////////////////////////////
+
+
+
+
+#ifdef DEBUG_BROADPHASE
+#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
+{
+	int numEdges = m_pHandles[0].m_maxEdges[axis];
+	printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
+
+	int i;
+	for (i=0;i<numEdges+1;i++)
+	{
+		Edge* pEdge = m_pEdges[axis] + i;
+		Handle* pHandlePrev = getHandle(pEdge->m_handle);
+		int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
+		char beginOrEnd;
+		beginOrEnd=pEdge->IsMax()?'E':'B';
+		printf("	[%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
+	}
+
+	if (checkCardinality)
+		assert(numEdges == m_numHandles*2+1);
+}
+#endif //DEBUG_BROADPHASE
+
+template <typename BP_FP_INT_TYPE>
+btBroadphaseProxy*	btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+		(void)shapeType;
+		BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy);
+		
+		Handle* handle = getHandle(handleId);
+		
+		if (m_raycastAccelerator)
+		{
+			btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0);
+			handle->m_dbvtProxy = rayProxy;
+		}
+		return handle;
+}
+
+
+
+template <typename BP_FP_INT_TYPE>
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
+	removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
+}
+
+template <typename BP_FP_INT_TYPE>
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
+{
+	Handle* handle = static_cast<Handle*>(proxy);
+	handle->m_aabbMin = aabbMin;
+	handle->m_aabbMax = aabbMax;
+	updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
+	if (m_raycastAccelerator)
+		m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
+
+}
+
+template <typename BP_FP_INT_TYPE>
+
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback);
+	} else
+	{
+		//choose axis?
+		BP_FP_INT_TYPE axis = 0;
+		//for each proxy
+		for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
+		{
+			if (m_pEdges[axis][i].IsMax())
+			{
+				rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
+			}
+		}
+	}
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+	aabbMin = pHandle->m_aabbMin;
+	aabbMax = pHandle->m_aabbMax;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	Handle* pHandle = static_cast<Handle*>(proxy);
+
+	unsigned short vecInMin[3];
+	unsigned short vecInMax[3];
+
+	vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
+	vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
+	vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
+	vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
+	vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
+	vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
+	
+	aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
+	aabbMin += m_worldAabbMin;
+	
+	aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
+	aabbMax += m_worldAabbMin;
+}
+
+
+
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator)
+:m_bpHandleMask(handleMask),
+m_handleSentinel(handleSentinel),
+m_pairCache(pairCache),
+m_userPairCallback(0),
+m_ownsPairCache(false),
+m_invalidPair(0),
+m_raycastAccelerator(0)
+{
+	BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
+
+	if (!m_pairCache)
+	{
+		void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
+		m_pairCache = new(ptr) btHashedOverlappingPairCache();
+		m_ownsPairCache = true;
+	}
+
+	if (!disableRaycastAccelerator)
+	{
+		m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase();//m_pairCache);
+		m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
+	}
+
+	//assert(bounds.HasVolume());
+
+	// init bounds
+	m_worldAabbMin = worldAabbMin;
+	m_worldAabbMax = worldAabbMax;
+
+	btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
+
+	BP_FP_INT_TYPE	maxInt = m_handleSentinel;
+
+	m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
+
+	// allocate handles buffer, using btAlignedAlloc, and put all handles on free list
+	m_pHandles = new Handle[maxHandles];
+	
+	m_maxHandles = maxHandles;
+	m_numHandles = 0;
+
+	// handle 0 is reserved as the null index, and is also used as the sentinel
+	m_firstFreeHandle = 1;
+	{
+		for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
+			m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+		m_pHandles[maxHandles - 1].SetNextFree(0);
+	}
+
+	{
+		// allocate edge buffers
+		for (int i = 0; i < 3; i++)
+		{
+			m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
+			m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
+		}
+	}
+	//removed overlap management
+
+	// make boundary sentinels
+	
+	m_pHandles[0].m_clientObject = 0;
+
+	for (int axis = 0; axis < 3; axis++)
+	{
+		m_pHandles[0].m_minEdges[axis] = 0;
+		m_pHandles[0].m_maxEdges[axis] = 1;
+
+		m_pEdges[axis][0].m_pos = 0;
+		m_pEdges[axis][0].m_handle = 0;
+		m_pEdges[axis][1].m_pos = m_handleSentinel;
+		m_pEdges[axis][1].m_handle = 0;
+#ifdef DEBUG_BROADPHASE
+		debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+	}
+
+}
+
+template <typename BP_FP_INT_TYPE>
+btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
+{
+	if (m_raycastAccelerator)
+		btAlignedFree (m_raycastAccelerator);
+
+	for (int i = 2; i >= 0; i--)
+	{
+		btAlignedFree(m_pEdgesRawPtr[i]);
+	}
+	delete [] m_pHandles;
+
+	if (m_ownsPairCache)
+	{
+		m_pairCache->~btOverlappingPairCache();
+		btAlignedFree(m_pairCache);
+	}
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
+{
+#ifdef OLD_CLAMPING_METHOD
+	///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax]
+	///see http://code.google.com/p/bullet/issues/detail?id=87
+	btPoint3 clampedPoint(point);
+	clampedPoint.setMax(m_worldAabbMin);
+	clampedPoint.setMin(m_worldAabbMax);
+	btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
+	out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
+	out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
+	out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
+#else
+	btVector3 v = (point - m_worldAabbMin) * m_quantize;
+	out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax);
+	out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax);
+	out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax);
+#endif //OLD_CLAMPING_METHOD
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
+{
+	assert(m_firstFreeHandle);
+
+	BP_FP_INT_TYPE handle = m_firstFreeHandle;
+	m_firstFreeHandle = getHandle(handle)->GetNextFree();
+	m_numHandles++;
+
+	return handle;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
+{
+	assert(handle > 0 && handle < m_maxHandles);
+
+	getHandle(handle)->SetNextFree(m_firstFreeHandle);
+	m_firstFreeHandle = handle;
+
+	m_numHandles--;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy)
+{
+	// quantize the bounds
+	BP_FP_INT_TYPE min[3], max[3];
+	quantize(min, aabbMin, 0);
+	quantize(max, aabbMax, 1);
+
+	// allocate a handle
+	BP_FP_INT_TYPE handle = allocHandle();
+	
+
+	Handle* pHandle = getHandle(handle);
+	
+	pHandle->m_uniqueId = static_cast<int>(handle);
+	//pHandle->m_pOverlaps = 0;
+	pHandle->m_clientObject = pOwner;
+	pHandle->m_collisionFilterGroup = collisionFilterGroup;
+	pHandle->m_collisionFilterMask = collisionFilterMask;
+	pHandle->m_multiSapParentProxy = multiSapProxy;
+
+	// compute current limit of edge arrays
+	BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
+
+	
+	// insert new edges just inside the max boundary edge
+	for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
+	{
+
+		m_pHandles[0].m_maxEdges[axis] += 2;
+
+		m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
+
+		m_pEdges[axis][limit - 1].m_pos = min[axis];
+		m_pEdges[axis][limit - 1].m_handle = handle;
+
+		m_pEdges[axis][limit].m_pos = max[axis];
+		m_pEdges[axis][limit].m_handle = handle;
+
+		pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
+		pHandle->m_maxEdges[axis] = limit;
+	}
+
+	// now sort the new edges to their correct position
+	sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
+	sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
+	sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
+	sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
+	sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
+	sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
+
+
+	return handle;
+}
+
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
+{
+
+	Handle* pHandle = getHandle(handle);
+
+	//explicitly remove the pairs containing the proxy
+	//we could do it also in the sortMinUp (passing true)
+	//todo: compare performance
+	if (!m_pairCache->hasDeferredRemoval())
+	{
+		m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
+	}
+
+	// compute current limit of edge arrays
+	int limit = static_cast<int>(m_numHandles * 2);
+	
+	int axis;
+
+	for (axis = 0;axis<3;axis++)
+	{
+		m_pHandles[0].m_maxEdges[axis] -= 2;
+	}
+
+	// remove the edges by sorting them up to the end of the list
+	for ( axis = 0; axis < 3; axis++)
+	{
+		Edge* pEdges = m_pEdges[axis];
+		BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
+		pEdges[max].m_pos = m_handleSentinel;
+
+		sortMaxUp(axis,max,dispatcher,false);
+
+
+		BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
+		pEdges[i].m_pos = m_handleSentinel;
+
+
+		sortMinUp(axis,i,dispatcher,false);
+
+		pEdges[limit-1].m_handle = 0;
+		pEdges[limit-1].m_pos = m_handleSentinel;
+		
+#ifdef DEBUG_BROADPHASE
+			debugPrintAxis(axis,false);
+#endif //DEBUG_BROADPHASE
+
+
+	}
+
+
+	// free the handle
+	freeHandle(handle);
+
+	
+}
+
+extern int gOverlappingPairs;
+//#include <stdio.h>
+
+template <typename BP_FP_INT_TYPE>
+void	btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+	if (m_pairCache->hasDeferredRemoval())
+	{
+	
+		btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+
+		
+		int i;
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;//callback->processOverlap(pair);
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+		//		m_overlappingPairArray.pop_back();
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				m_invalidPair++;
+				gOverlappingPairs--;
+			} 
+			
+		}
+
+	///if you don't like to skip the invalid pairs in the array, execute following code:
+	#define CLEAN_INVALID_PAIRS 1
+	#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+	#endif//CLEAN_INVALID_PAIRS
+		
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+	}
+
+
+
+	
+
+}
+
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	const Handle* pHandleA = static_cast<Handle*>(proxy0);
+	const Handle* pHandleB = static_cast<Handle*>(proxy1);
+	
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	for (int axis = 0; axis < 3; axis++)
+	{ 
+		if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
+			pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
+		{ 
+			return false; 
+		} 
+	} 
+	return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1)
+{
+	//optimization 1: check the array index (memory address), instead of the m_pos
+
+	if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || 
+		pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
+		pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
+		pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) 
+	{ 
+		return false; 
+	} 
+	return true;
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax,btDispatcher* dispatcher)
+{
+//	assert(bounds.IsFinite());
+	//assert(bounds.HasVolume());
+
+	Handle* pHandle = getHandle(handle);
+
+	// quantize the new bounds
+	BP_FP_INT_TYPE min[3], max[3];
+	quantize(min, aabbMin, 0);
+	quantize(max, aabbMax, 1);
+
+	// update changed edges
+	for (int axis = 0; axis < 3; axis++)
+	{
+		BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
+		BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
+
+		int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
+		int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
+
+		m_pEdges[axis][emin].m_pos = min[axis];
+		m_pEdges[axis][emax].m_pos = max[axis];
+
+		// expand (only adds overlaps)
+		if (dmin < 0)
+			sortMinDown(axis, emin,dispatcher,true);
+
+		if (dmax > 0)
+			sortMaxUp(axis, emax,dispatcher,true);
+
+		// shrink (only removes overlaps)
+		if (dmin > 0)
+			sortMinUp(axis, emin,dispatcher,true);
+
+		if (dmax < 0)
+			sortMaxDown(axis, emax,dispatcher,true);
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+	}
+
+	
+}
+
+
+
+
+// sorting a min edge downwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (pPrev->IsMax())
+		{
+			// if previous edge is a maximum check the bounds and add an overlap if necessary
+			const int axis1 = (1  << axis) & 3;
+			const int axis2 = (1  << axis1) & 3;
+			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
+			{
+				m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
+				if (m_userPairCallback)
+					m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
+
+				//AddOverlap(pEdge->m_handle, pPrev->m_handle);
+
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_maxEdges[axis]++;
+		}
+		else
+			pHandlePrev->m_minEdges[axis]++;
+
+		pHandleEdge->m_minEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a min edge upwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		if (pNext->IsMax())
+		{
+			Handle* handle0 = getHandle(pEdge->m_handle);
+			Handle* handle1 = getHandle(pNext->m_handle);
+			const int axis1 = (1  << axis) & 3;
+			const int axis2 = (1  << axis1) & 3;
+			
+			// if next edge is maximum remove any overlap between the two handles
+			if (updateOverlaps 
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+				&& testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+				)
+			{
+				
+
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);	
+				if (m_userPairCallback)
+					m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+				
+			}
+
+
+			// update edge reference in other handle
+			pHandleNext->m_maxEdges[axis]--;
+		}
+		else
+			pHandleNext->m_minEdges[axis]--;
+
+		pHandleEdge->m_minEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+
+
+}
+
+// sorting a max edge downwards can only ever *remove* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
+{
+
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pPrev = pEdge - 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pEdge->m_pos < pPrev->m_pos)
+	{
+		Handle* pHandlePrev = getHandle(pPrev->m_handle);
+
+		if (!pPrev->IsMax())
+		{
+			// if previous edge was a minimum remove any overlap between the two handles
+			Handle* handle0 = getHandle(pEdge->m_handle);
+			Handle* handle1 = getHandle(pPrev->m_handle);
+			const int axis1 = (1  << axis) & 3;
+			const int axis2 = (1  << axis1) & 3;
+
+			if (updateOverlaps  
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+				&& testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+				)
+			{
+				//this is done during the overlappingpairarray iteration/narrowphase collision
+
+				
+				m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
+				if (m_userPairCallback)
+					m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
+			
+
+
+			}
+
+			// update edge reference in other handle
+			pHandlePrev->m_minEdges[axis]++;;
+		}
+		else
+			pHandlePrev->m_maxEdges[axis]++;
+
+		pHandleEdge->m_maxEdges[axis]--;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pPrev;
+		*pPrev = swap;
+
+		// decrement
+		pEdge--;
+		pPrev--;
+	}
+
+	
+#ifdef DEBUG_BROADPHASE
+	debugPrintAxis(axis);
+#endif //DEBUG_BROADPHASE
+
+}
+
+// sorting a max edge upwards can only ever *add* overlaps
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
+{
+	Edge* pEdge = m_pEdges[axis] + edge;
+	Edge* pNext = pEdge + 1;
+	Handle* pHandleEdge = getHandle(pEdge->m_handle);
+
+	while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
+	{
+		Handle* pHandleNext = getHandle(pNext->m_handle);
+
+		const int axis1 = (1  << axis) & 3;
+		const int axis2 = (1  << axis1) & 3;
+
+		if (!pNext->IsMax())
+		{
+			// if next edge is a minimum check the bounds and add an overlap if necessary
+			if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
+			{
+				Handle* handle0 = getHandle(pEdge->m_handle);
+				Handle* handle1 = getHandle(pNext->m_handle);
+				m_pairCache->addOverlappingPair(handle0,handle1);
+				if (m_userPairCallback)
+					m_userPairCallback->addOverlappingPair(handle0,handle1);
+			}
+
+			// update edge reference in other handle
+			pHandleNext->m_minEdges[axis]--;
+		}
+		else
+			pHandleNext->m_maxEdges[axis]--;
+
+		pHandleEdge->m_maxEdges[axis]++;
+
+		// swap the edges
+		Edge swap = *pEdge;
+		*pEdge = *pNext;
+		*pNext = swap;
+
+		// increment
+		pEdge++;
+		pNext++;
+	}
+	
+}
+
+
+
+////////////////////////////////////////////////////////////////////
+
+
+/// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
+/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats.
+/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance.
+class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
+{
+public:
+
+	btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+};
+
+/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
+/// This comes at the cost of more memory per handle, and a bit slower performance.
+/// It uses arrays rather then lists for storage of the 3 axis.
+class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
+{
+public:
+
+	bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+};
+
+#endif
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h	(revision 1963)
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef		BROADPHASE_INTERFACE_H
+#define 	BROADPHASE_INTERFACE_H
+
+
+
+struct btDispatcherInfo;
+class btDispatcher;
+#include "btBroadphaseProxy.h"
+class btOverlappingPairCache;
+
+struct	btBroadphaseRayCallback
+{
+	virtual ~btBroadphaseRayCallback() {}
+	virtual bool	process(const btBroadphaseProxy* proxy) = 0;
+};
+
+#include "LinearMath/btVector3.h"
+
+///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
+///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase.
+///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class.
+class btBroadphaseInterface
+{
+public:
+	virtual ~btBroadphaseInterface() {}
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0;
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0;
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0;
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0;
+
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback) = 0;
+
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher)=0;
+
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()=0;
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const =0;
+
+	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+	///will add some transform later
+	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+	virtual void	printStats() = 0;
+
+};
+
+#endif //BROADPHASE_INTERFACE_H
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp	(revision 1963)
@@ -0,0 +1,17 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBroadphaseProxy.h"
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h	(revision 1963)
@@ -0,0 +1,244 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BROADPHASE_PROXY_H
+#define BROADPHASE_PROXY_H
+
+#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+/// btDispatcher uses these types
+/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
+/// to facilitate type checking
+enum BroadphaseNativeTypes
+{
+	// polyhedral convex shapes
+	BOX_SHAPE_PROXYTYPE,
+	TRIANGLE_SHAPE_PROXYTYPE,
+	TETRAHEDRAL_SHAPE_PROXYTYPE,
+	CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
+	CONVEX_HULL_SHAPE_PROXYTYPE,
+	CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE,
+//implicit convex shapes
+IMPLICIT_CONVEX_SHAPES_START_HERE,
+	SPHERE_SHAPE_PROXYTYPE,
+	MULTI_SPHERE_SHAPE_PROXYTYPE,
+	CAPSULE_SHAPE_PROXYTYPE,
+	CONE_SHAPE_PROXYTYPE,
+	CONVEX_SHAPE_PROXYTYPE,
+	CYLINDER_SHAPE_PROXYTYPE,
+	UNIFORM_SCALING_SHAPE_PROXYTYPE,
+	MINKOWSKI_SUM_SHAPE_PROXYTYPE,
+	MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+//concave shapes
+CONCAVE_SHAPES_START_HERE,
+	//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+	TRIANGLE_MESH_SHAPE_PROXYTYPE,
+	SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE,
+	///used for demo integration FAST/Swift collision library and Bullet
+	FAST_CONCAVE_MESH_PROXYTYPE,
+	//terrain
+	TERRAIN_SHAPE_PROXYTYPE,
+///Used for GIMPACT Trimesh integration
+	GIMPACT_SHAPE_PROXYTYPE,
+///Multimaterial mesh
+    MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE,
+	
+	EMPTY_SHAPE_PROXYTYPE,
+	STATIC_PLANE_PROXYTYPE,
+CONCAVE_SHAPES_END_HERE,
+
+	COMPOUND_SHAPE_PROXYTYPE,
+
+	SOFTBODY_SHAPE_PROXYTYPE,
+
+	INVALID_SHAPE_PROXYTYPE,
+
+	MAX_BROADPHASE_COLLISION_TYPES
+	
+};
+
+
+///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. 
+///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody.
+ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
+{
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	///optional filtering to cull potential collisions
+	enum CollisionFilterGroups
+	{
+	        DefaultFilter = 1,
+	        StaticFilter = 2,
+	        KinematicFilter = 4,
+	        DebrisFilter = 8,
+			SensorTrigger = 16,
+	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+	};
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax;
+
+	//Usually the client btCollisionObject or Rigidbody class
+	void*	m_clientObject;
+
+	short int m_collisionFilterGroup;
+	short int m_collisionFilterMask;
+
+	void*	m_multiSapParentProxy;		
+
+
+	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	SIMD_FORCE_INLINE int getUid() const
+	{
+		return m_uniqueId;
+	}
+
+	//used for memory pools
+	btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
+	{
+	}
+
+	btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
+		:m_aabbMin(aabbMin),
+		m_aabbMax(aabbMax),
+		m_clientObject(userPtr),
+		m_collisionFilterGroup(collisionFilterGroup),
+		m_collisionFilterMask(collisionFilterMask)
+	{
+		m_multiSapParentProxy = multiSapParentProxy;
+	}
+
+	
+
+	static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType)
+	{
+		return (proxyType  < IMPLICIT_CONVEX_SHAPES_START_HERE);
+	}
+
+	static SIMD_FORCE_INLINE bool	isConvex(int proxyType)
+	{
+		return (proxyType < CONCAVE_SHAPES_START_HERE);
+	}
+
+	static SIMD_FORCE_INLINE bool	isConcave(int proxyType)
+	{
+		return ((proxyType > CONCAVE_SHAPES_START_HERE) &&
+			(proxyType < CONCAVE_SHAPES_END_HERE));
+	}
+	static SIMD_FORCE_INLINE bool	isCompound(int proxyType)
+	{
+		return (proxyType == COMPOUND_SHAPE_PROXYTYPE);
+	}
+	static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
+	{
+		return (proxyType == STATIC_PLANE_PROXYTYPE);
+	}
+	
+}
+;
+
+class btCollisionAlgorithm;
+
+struct btBroadphaseProxy;
+
+
+
+///The btBroadphasePair class contains a pair of aabb-overlapping objects.
+///A btDispatcher can search a btCollisionAlgorithm that performs exact/narrowphase collision detection on the actual collision shapes.
+ATTRIBUTE_ALIGNED16(struct) btBroadphasePair
+{
+	btBroadphasePair ()
+		:
+	m_pProxy0(0),
+		m_pProxy1(0),
+		m_algorithm(0),
+		m_userInfo(0)
+	{
+	}
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btBroadphasePair(const btBroadphasePair& other)
+		:		m_pProxy0(other.m_pProxy0),
+				m_pProxy1(other.m_pProxy1),
+				m_algorithm(other.m_algorithm),
+				m_userInfo(other.m_userInfo)
+	{
+	}
+	btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+	{
+
+		//keep them sorted, so the std::set operations work
+		if (&proxy0 < &proxy1)
+        { 
+            m_pProxy0 = &proxy0; 
+            m_pProxy1 = &proxy1; 
+        }
+        else 
+        { 
+			m_pProxy0 = &proxy1; 
+            m_pProxy1 = &proxy0; 
+        }
+
+		m_algorithm = 0;
+		m_userInfo = 0;
+
+	}
+	
+	btBroadphaseProxy* m_pProxy0;
+	btBroadphaseProxy* m_pProxy1;
+	
+	mutable btCollisionAlgorithm* m_algorithm;
+	mutable void* m_userInfo;
+
+};
+
+/*
+//comparison for set operation, see Solid DT_Encounter
+SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) 
+{ 
+    return a.m_pProxy0 < b.m_pProxy0 || 
+        (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); 
+}
+*/
+
+
+
+class btBroadphasePairSortPredicate
+{
+	public:
+
+		bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
+		{
+			 return a.m_pProxy0 > b.m_pProxy0 || 
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
+				(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm); 
+		}
+};
+
+
+SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) 
+{
+	 return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1);
+}
+
+
+#endif //BROADPHASE_PROXY_H
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,23 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCollisionAlgorithm.h"
+#include "btDispatcher.h"
+
+btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+{
+	m_dispatcher = ci.m_dispatcher1;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_ALGORITHM_H
+#define COLLISION_ALGORITHM_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btBroadphaseProxy;
+class btDispatcher;
+class btManifoldResult;
+class btCollisionObject;
+struct btDispatcherInfo;
+class	btPersistentManifold;
+
+typedef btAlignedObjectArray<btPersistentManifold*>	btManifoldArray;
+
+struct btCollisionAlgorithmConstructionInfo
+{
+	btCollisionAlgorithmConstructionInfo()
+		:m_dispatcher1(0),
+		m_manifold(0)
+	{
+	}
+	btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp)
+		:m_dispatcher1(dispatcher)
+	{
+		(void)temp;
+	}
+
+	btDispatcher*	m_dispatcher1;
+	btPersistentManifold*	m_manifold;
+
+	int	getDispatcherId();
+
+};
+
+
+///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher.
+///It is persistent over frames
+class btCollisionAlgorithm
+{
+
+protected:
+
+	btDispatcher*	m_dispatcher;
+
+protected:
+	int	getDispatcherId();
+	
+public:
+
+	btCollisionAlgorithm() {};
+
+	btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+	virtual ~btCollisionAlgorithm() {};
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray) = 0;
+};
+
+
+#endif //COLLISION_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp	(revision 1963)
@@ -0,0 +1,1289 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#include "btDbvt.h"
+
+//
+typedef btAlignedObjectArray<btDbvtNode*>			tNodeArray;
+typedef btAlignedObjectArray<const btDbvtNode*>	tConstNodeArray;
+
+//
+struct btDbvtNodeEnumerator : btDbvt::ICollide
+{
+	tConstNodeArray	nodes;
+	void Process(const btDbvtNode* n) { nodes.push_back(n); }
+};
+
+//
+static DBVT_INLINE int			indexof(const btDbvtNode* node)
+{
+	return(node->parent->childs[1]==node);
+}
+
+//
+static DBVT_INLINE btDbvtVolume	merge(	const btDbvtVolume& a,
+									  const btDbvtVolume& b)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+	DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)];
+	btDbvtVolume&	res=*(btDbvtVolume*)locals;
+#else
+	btDbvtVolume	res;
+#endif
+	Merge(a,b,res);
+	return(res);
+}
+
+// volume+edge lengths
+static DBVT_INLINE btScalar		size(const btDbvtVolume& a)
+{
+	const btVector3	edges=a.Lengths();
+	return(	edges.x()*edges.y()*edges.z()+
+		edges.x()+edges.y()+edges.z());
+}
+
+//
+static void						getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
+{
+	if(node->isinternal())
+	{
+		getmaxdepth(node->childs[0],depth+1,maxdepth);
+		getmaxdepth(node->childs[0],depth+1,maxdepth);
+	} else maxdepth=btMax(maxdepth,depth);
+}
+
+//
+static DBVT_INLINE void			deletenode(	btDbvt* pdbvt,
+										   btDbvtNode* node)
+{
+	btAlignedFree(pdbvt->m_free);
+	pdbvt->m_free=node;
+}
+
+//
+static void						recursedeletenode(	btDbvt* pdbvt,
+												  btDbvtNode* node)
+{
+	if(!node->isleaf())
+	{
+		recursedeletenode(pdbvt,node->childs[0]);
+		recursedeletenode(pdbvt,node->childs[1]);
+	}
+	if(node==pdbvt->m_root) pdbvt->m_root=0;
+	deletenode(pdbvt,node);
+}
+
+//
+static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
+										   btDbvtNode* parent,
+										   void* data)
+{
+	btDbvtNode*	node;
+	if(pdbvt->m_free)
+	{ node=pdbvt->m_free;pdbvt->m_free=0; }
+	else
+	{ node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
+	node->parent	=	parent;
+	node->data		=	data;
+	node->childs[1]	=	0;
+	return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
+										   btDbvtNode* parent,
+										   const btDbvtVolume& volume,
+										   void* data)
+{
+	btDbvtNode*	node=createnode(pdbvt,parent,data);
+	node->volume=volume;
+	return(node);
+}
+
+//
+static DBVT_INLINE btDbvtNode*	createnode(	btDbvt* pdbvt,
+										   btDbvtNode* parent,
+										   const btDbvtVolume& volume0,
+										   const btDbvtVolume& volume1,
+										   void* data)
+{
+	btDbvtNode*	node=createnode(pdbvt,parent,data);
+	Merge(volume0,volume1,node->volume);
+	return(node);
+}
+
+//
+static void						insertleaf(	btDbvt* pdbvt,
+										   btDbvtNode* root,
+										   btDbvtNode* leaf)
+{
+	if(!pdbvt->m_root)
+	{
+		pdbvt->m_root	=	leaf;
+		leaf->parent	=	0;
+	}
+	else
+	{
+		if(!root->isleaf())
+		{
+			do	{
+				root=root->childs[Select(	leaf->volume,
+					root->childs[0]->volume,
+					root->childs[1]->volume)];
+			} while(!root->isleaf());
+		}
+		btDbvtNode*	prev=root->parent;
+		btDbvtNode*	node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
+		if(prev)
+		{
+			prev->childs[indexof(root)]	=	node;
+			node->childs[0]				=	root;root->parent=node;
+			node->childs[1]				=	leaf;leaf->parent=node;
+			do	{
+				if(!prev->volume.Contain(node->volume))
+					Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+				else
+					break;
+				node=prev;
+			} while(0!=(prev=node->parent));
+		}
+		else
+		{
+			node->childs[0]	=	root;root->parent=node;
+			node->childs[1]	=	leaf;leaf->parent=node;
+			pdbvt->m_root	=	node;
+		}
+	}
+}
+
+//
+static btDbvtNode*				removeleaf(	btDbvt* pdbvt,
+										   btDbvtNode* leaf)
+{
+	if(leaf==pdbvt->m_root)
+	{
+		pdbvt->m_root=0;
+		return(0);
+	}
+	else
+	{
+		btDbvtNode*	parent=leaf->parent;
+		btDbvtNode*	prev=parent->parent;
+		btDbvtNode*	sibling=parent->childs[1-indexof(leaf)];			
+		if(prev)
+		{
+			prev->childs[indexof(parent)]=sibling;
+			sibling->parent=prev;
+			deletenode(pdbvt,parent);
+			while(prev)
+			{
+				const btDbvtVolume	pb=prev->volume;
+				Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+				if(NotEqual(pb,prev->volume))
+				{
+					prev=prev->parent;
+				} else break;
+			}
+			return(prev?prev:pdbvt->m_root);
+		}
+		else
+		{								
+			pdbvt->m_root=sibling;
+			sibling->parent=0;
+			deletenode(pdbvt,parent);
+			return(pdbvt->m_root);
+		}			
+	}
+}
+
+//
+static void						fetchleaves(btDbvt* pdbvt,
+											btDbvtNode* root,
+											tNodeArray& leaves,
+											int depth=-1)
+{
+	if(root->isinternal()&&depth)
+	{
+		fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
+		fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
+		deletenode(pdbvt,root);
+	}
+	else
+	{
+		leaves.push_back(root);
+	}
+}
+
+//
+static void						split(	const tNodeArray& leaves,
+									  tNodeArray& left,
+									  tNodeArray& right,
+									  const btVector3& org,
+									  const btVector3& axis)
+{
+	left.resize(0);
+	right.resize(0);
+	for(int i=0,ni=leaves.size();i<ni;++i)
+	{
+		if(dot(axis,leaves[i]->volume.Center()-org)<0)
+			left.push_back(leaves[i]);
+		else
+			right.push_back(leaves[i]);
+	}
+}
+
+//
+static btDbvtVolume				bounds(	const tNodeArray& leaves)
+{
+#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
+	DBVT_ALIGN char	locals[sizeof(btDbvtVolume)];
+	btDbvtVolume&	volume=*(btDbvtVolume*)locals;
+	volume=leaves[0]->volume;
+#else
+	btDbvtVolume volume=leaves[0]->volume;
+#endif
+	for(int i=1,ni=leaves.size();i<ni;++i)
+	{
+		Merge(volume,leaves[i]->volume,volume);
+	}
+	return(volume);
+}
+
+//
+static void						bottomup(	btDbvt* pdbvt,
+										 tNodeArray& leaves)
+{
+	while(leaves.size()>1)
+	{
+		btScalar	minsize=SIMD_INFINITY;
+		int			minidx[2]={-1,-1};
+		for(int i=0;i<leaves.size();++i)
+		{
+			for(int j=i+1;j<leaves.size();++j)
+			{
+				const btScalar	sz=size(merge(leaves[i]->volume,leaves[j]->volume));
+				if(sz<minsize)
+				{
+					minsize		=	sz;
+					minidx[0]	=	i;
+					minidx[1]	=	j;
+				}
+			}
+		}
+		btDbvtNode*	n[]	=	{leaves[minidx[0]],leaves[minidx[1]]};
+		btDbvtNode*	p	=	createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
+		p->childs[0]		=	n[0];
+		p->childs[1]		=	n[1];
+		n[0]->parent		=	p;
+		n[1]->parent		=	p;
+		leaves[minidx[0]]	=	p;
+		leaves.swap(minidx[1],leaves.size()-1);
+		leaves.pop_back();
+	}
+}
+
+//
+static btDbvtNode*			topdown(btDbvt* pdbvt,
+									tNodeArray& leaves,
+									int bu_treshold)
+{
+	static const btVector3	axis[]={btVector3(1,0,0),
+		btVector3(0,1,0),
+		btVector3(0,0,1)};
+	if(leaves.size()>1)
+	{
+		if(leaves.size()>bu_treshold)
+		{
+			const btDbvtVolume	vol=bounds(leaves);
+			const btVector3			org=vol.Center();
+			tNodeArray				sets[2];
+			int						bestaxis=-1;
+			int						bestmidp=leaves.size();
+			int						splitcount[3][2]={{0,0},{0,0},{0,0}};
+			int i;
+			for( i=0;i<leaves.size();++i)
+			{
+				const btVector3	x=leaves[i]->volume.Center()-org;
+				for(int j=0;j<3;++j)
+				{
+					++splitcount[j][dot(x,axis[j])>0?1:0];
+				}
+			}
+			for( i=0;i<3;++i)
+			{
+				if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
+				{
+					const int	midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
+					if(midp<bestmidp)
+					{
+						bestaxis=i;
+						bestmidp=midp;
+					}
+				}
+			}
+			if(bestaxis>=0)
+			{
+				sets[0].reserve(splitcount[bestaxis][0]);
+				sets[1].reserve(splitcount[bestaxis][1]);
+				split(leaves,sets[0],sets[1],org,axis[bestaxis]);
+			}
+			else
+			{
+				sets[0].reserve(leaves.size()/2+1);
+				sets[1].reserve(leaves.size()/2);
+				for(int i=0,ni=leaves.size();i<ni;++i)
+				{
+					sets[i&1].push_back(leaves[i]);
+				}
+			}
+			btDbvtNode*	node=createnode(pdbvt,0,vol,0);
+			node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
+			node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
+			node->childs[0]->parent=node;
+			node->childs[1]->parent=node;
+			return(node);
+		}
+		else
+		{
+			bottomup(pdbvt,leaves);
+			return(leaves[0]);
+		}
+	}
+	return(leaves[0]);
+}
+
+//
+static DBVT_INLINE btDbvtNode*	sort(btDbvtNode* n,btDbvtNode*& r)
+{
+	btDbvtNode*	p=n->parent;
+	btAssert(n->isinternal());
+	if(p>n)
+	{
+		const int		i=indexof(n);
+		const int		j=1-i;
+		btDbvtNode*	s=p->childs[j];
+		btDbvtNode*	q=p->parent;
+		btAssert(n==p->childs[i]);
+		if(q) q->childs[indexof(p)]=n; else r=n;
+		s->parent=n;
+		p->parent=n;
+		n->parent=q;
+		p->childs[0]=n->childs[0];
+		p->childs[1]=n->childs[1];
+		n->childs[0]->parent=p;
+		n->childs[1]->parent=p;
+		n->childs[i]=p;
+		n->childs[j]=s;
+		btSwap(p->volume,n->volume);
+		return(p);
+	}
+	return(n);
+}
+
+//
+static DBVT_INLINE btDbvtNode*	walkup(btDbvtNode* n,int count)
+{
+	while(n&&(count--)) n=n->parent;
+	return(n);
+}
+
+//
+// Api
+//
+
+//
+btDbvt::btDbvt()
+{
+	m_root		=	0;
+	m_free		=	0;
+	m_lkhd		=	-1;
+	m_leaves	=	0;
+	m_opath		=	0;
+}
+
+//
+btDbvt::~btDbvt()
+{
+	clear();
+}
+
+//
+void			btDbvt::clear()
+{
+	if(m_root)	recursedeletenode(this,m_root);
+	btAlignedFree(m_free);
+	m_free=0;
+}
+
+//
+void			btDbvt::optimizeBottomUp()
+{
+	if(m_root)
+	{
+		tNodeArray leaves;
+		leaves.reserve(m_leaves);
+		fetchleaves(this,m_root,leaves);
+		bottomup(this,leaves);
+		m_root=leaves[0];
+	}
+}
+
+//
+void			btDbvt::optimizeTopDown(int bu_treshold)
+{
+	if(m_root)
+	{
+		tNodeArray	leaves;
+		leaves.reserve(m_leaves);
+		fetchleaves(this,m_root,leaves);
+		m_root=topdown(this,leaves,bu_treshold);
+	}
+}
+
+//
+void			btDbvt::optimizeIncremental(int passes)
+{
+	if(passes<0) passes=m_leaves;
+	if(m_root&&(passes>0))
+	{
+		do	{
+			btDbvtNode*		node=m_root;
+			unsigned	bit=0;
+			while(node->isinternal())
+			{
+				node=sort(node,m_root)->childs[(m_opath>>bit)&1];
+				bit=(bit+1)&(sizeof(unsigned)*8-1);
+			}
+			update(node);
+			++m_opath;
+		} while(--passes);
+	}
+}
+
+//
+btDbvtNode*	btDbvt::insert(const btDbvtVolume& volume,void* data)
+{
+	btDbvtNode*	leaf=createnode(this,0,volume,data);
+	insertleaf(this,m_root,leaf);
+	++m_leaves;
+	return(leaf);
+}
+
+//
+void			btDbvt::update(btDbvtNode* leaf,int lookahead)
+{
+	btDbvtNode*	root=removeleaf(this,leaf);
+	if(root)
+	{
+		if(lookahead>=0)
+		{
+			for(int i=0;(i<lookahead)&&root->parent;++i)
+			{
+				root=root->parent;
+			}
+		} else root=m_root;
+	}
+	insertleaf(this,root,leaf);
+}
+
+//
+void			btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume)
+{
+	btDbvtNode*	root=removeleaf(this,leaf);
+	if(root)
+	{
+		if(m_lkhd>=0)
+		{
+			for(int i=0;(i<m_lkhd)&&root->parent;++i)
+			{
+				root=root->parent;
+			}
+		} else root=m_root;
+	}
+	leaf->volume=volume;
+	insertleaf(this,root,leaf);
+}
+
+//
+bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.Expand(btVector3(margin,margin,margin));
+	volume.SignedExpand(velocity);
+	update(leaf,volume);
+	return(true);
+}
+
+//
+bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.SignedExpand(velocity);
+	update(leaf,volume);
+	return(true);
+}
+
+//
+bool			btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.Expand(btVector3(margin,margin,margin));
+	update(leaf,volume);
+	return(true);
+}
+
+//
+void			btDbvt::remove(btDbvtNode* leaf)
+{
+	removeleaf(this,leaf);
+	deletenode(this,leaf);
+	--m_leaves;
+}
+
+//
+void			btDbvt::write(IWriter* iwriter) const
+{
+	btDbvtNodeEnumerator	nodes;
+	nodes.nodes.reserve(m_leaves*2);
+	enumNodes(m_root,nodes);
+	iwriter->Prepare(m_root,nodes.nodes.size());
+	for(int i=0;i<nodes.nodes.size();++i)
+	{
+		const btDbvtNode* n=nodes.nodes[i];
+		int			p=-1;
+		if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
+		if(n->isinternal())
+		{
+			const int	c0=nodes.nodes.findLinearSearch(n->childs[0]);
+			const int	c1=nodes.nodes.findLinearSearch(n->childs[1]);
+			iwriter->WriteNode(n,i,p,c0,c1);
+		}
+		else
+		{
+			iwriter->WriteLeaf(n,i,p);
+		}	
+	}
+}
+
+//
+void			btDbvt::clone(btDbvt& dest,IClone* iclone) const
+{
+	dest.clear();
+	if(m_root!=0)
+	{	
+		btAlignedObjectArray<sStkCLN>	stack;
+		stack.reserve(m_leaves);
+		stack.push_back(sStkCLN(m_root,0));
+		do	{
+			const int		i=stack.size()-1;
+			const sStkCLN	e=stack[i];
+			btDbvtNode*			n=createnode(&dest,e.parent,e.node->volume,e.node->data);
+			stack.pop_back();
+			if(e.parent!=0)
+				e.parent->childs[i&1]=n;
+			else
+				dest.m_root=n;
+			if(e.node->isinternal())
+			{
+				stack.push_back(sStkCLN(e.node->childs[0],n));
+				stack.push_back(sStkCLN(e.node->childs[1],n));
+			}
+			else
+			{
+				iclone->CloneLeaf(n);
+			}
+		} while(stack.size()>0);
+	}
+}
+
+//
+int				btDbvt::maxdepth(const btDbvtNode* node)
+{
+	int	depth=0;
+	if(node) getmaxdepth(node,1,depth);
+	return(depth);
+}
+
+//
+int				btDbvt::countLeaves(const btDbvtNode* node)
+{
+	if(node->isinternal())
+		return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
+	else
+		return(1);
+}
+
+//
+void			btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
+{
+	if(node->isinternal())
+	{
+		extractLeaves(node->childs[0],leaves);
+		extractLeaves(node->childs[1],leaves);
+	}
+	else
+	{
+		leaves.push_back(node);
+	}	
+}
+
+//
+#if DBVT_ENABLE_BENCHMARK
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "LinearMath/btQuickProf.h"
+
+/*
+q6600,2.4ghz
+
+/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
+/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
+/Fo"..\..\out\release8\build\libbulletcollision\\"
+/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
+/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
+
+Benchmarking dbvt...
+World scale: 100.000000
+Extents base: 1.000000
+Extents range: 4.000000
+Leaves: 8192
+sizeof(btDbvtVolume): 32 bytes
+sizeof(btDbvtNode):   44 bytes
+[1] btDbvtVolume intersections: 3499 ms (-1%)
+[2] btDbvtVolume merges: 1934 ms (0%)
+[3] btDbvt::collideTT: 5485 ms (-21%)
+[4] btDbvt::collideTT self: 2814 ms (-20%)
+[5] btDbvt::collideTT xform: 7379 ms (-1%)
+[6] btDbvt::collideTT xform,self: 7270 ms (-2%)
+[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s)
+[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
+[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
+[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
+[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
+[12] btDbvtVolume notequal: 3659 ms (0%)
+[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
+[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
+[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
+[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
+[17] btDbvtVolume select: 3419 ms (0%)
+*/
+
+struct btDbvtBenchmark
+{
+	struct NilPolicy : btDbvt::ICollide
+	{
+		NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)		{}
+		void	Process(const btDbvtNode*,const btDbvtNode*)				{ ++m_pcount; }
+		void	Process(const btDbvtNode*)									{ ++m_pcount; }
+		void	Process(const btDbvtNode*,btScalar depth)
+		{
+			++m_pcount;
+			if(m_checksort)
+			{ if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
+		}
+		int			m_pcount;
+		btScalar	m_depth;
+		bool		m_checksort;
+	};
+	struct P14 : btDbvt::ICollide
+	{
+		struct Node
+		{
+			const btDbvtNode*	leaf;
+			btScalar			depth;
+		};
+		void Process(const btDbvtNode* leaf,btScalar depth)
+		{
+			Node	n;
+			n.leaf	=	leaf;
+			n.depth	=	depth;
+		}
+		static int sortfnc(const Node& a,const Node& b)
+		{
+			if(a.depth<b.depth) return(+1);
+			if(a.depth>b.depth) return(-1);
+			return(0);
+		}
+		btAlignedObjectArray<Node>		m_nodes;
+	};
+	struct P15 : btDbvt::ICollide
+	{
+		struct Node
+		{
+			const btDbvtNode*	leaf;
+			btScalar			depth;
+		};
+		void Process(const btDbvtNode* leaf)
+		{
+			Node	n;
+			n.leaf	=	leaf;
+			n.depth	=	dot(leaf->volume.Center(),m_axis);
+		}
+		static int sortfnc(const Node& a,const Node& b)
+		{
+			if(a.depth<b.depth) return(+1);
+			if(a.depth>b.depth) return(-1);
+			return(0);
+		}
+		btAlignedObjectArray<Node>		m_nodes;
+		btVector3						m_axis;
+	};
+	static btScalar			RandUnit()
+	{
+		return(rand()/(btScalar)RAND_MAX);
+	}
+	static btVector3		RandVector3()
+	{
+		return(btVector3(RandUnit(),RandUnit(),RandUnit()));
+	}
+	static btVector3		RandVector3(btScalar cs)
+	{
+		return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
+	}
+	static btDbvtVolume	RandVolume(btScalar cs,btScalar eb,btScalar es)
+	{
+		return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
+	}
+	static btTransform		RandTransform(btScalar cs)
+	{
+		btTransform	t;
+		t.setOrigin(RandVector3(cs));
+		t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
+		return(t);
+	}
+	static void				RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
+	{
+		dbvt.clear();
+		for(int i=0;i<leaves;++i)
+		{
+			dbvt.insert(RandVolume(cs,eb,es),0);
+		}
+	}
+};
+
+void			btDbvt::benchmark()
+{
+	static const btScalar	cfgVolumeCenterScale		=	100;
+	static const btScalar	cfgVolumeExentsBase			=	1;
+	static const btScalar	cfgVolumeExentsScale		=	4;
+	static const int		cfgLeaves					=	8192;
+	static const bool		cfgEnable					=	true;
+
+	//[1] btDbvtVolume intersections
+	bool					cfgBenchmark1_Enable		=	cfgEnable;
+	static const int		cfgBenchmark1_Iterations	=	8;
+	static const int		cfgBenchmark1_Reference		=	3499;
+	//[2] btDbvtVolume merges
+	bool					cfgBenchmark2_Enable		=	cfgEnable;
+	static const int		cfgBenchmark2_Iterations	=	4;
+	static const int		cfgBenchmark2_Reference		=	1945;
+	//[3] btDbvt::collideTT
+	bool					cfgBenchmark3_Enable		=	cfgEnable;
+	static const int		cfgBenchmark3_Iterations	=	512;
+	static const int		cfgBenchmark3_Reference		=	5485;
+	//[4] btDbvt::collideTT self
+	bool					cfgBenchmark4_Enable		=	cfgEnable;
+	static const int		cfgBenchmark4_Iterations	=	512;
+	static const int		cfgBenchmark4_Reference		=	2814;
+	//[5] btDbvt::collideTT xform
+	bool					cfgBenchmark5_Enable		=	cfgEnable;
+	static const int		cfgBenchmark5_Iterations	=	512;
+	static const btScalar	cfgBenchmark5_OffsetScale	=	2;
+	static const int		cfgBenchmark5_Reference		=	7379;
+	//[6] btDbvt::collideTT xform,self
+	bool					cfgBenchmark6_Enable		=	cfgEnable;
+	static const int		cfgBenchmark6_Iterations	=	512;
+	static const btScalar	cfgBenchmark6_OffsetScale	=	2;
+	static const int		cfgBenchmark6_Reference		=	7270;
+	//[7] btDbvt::rayTest
+	bool					cfgBenchmark7_Enable		=	cfgEnable;
+	static const int		cfgBenchmark7_Passes		=	32;
+	static const int		cfgBenchmark7_Iterations	=	65536;
+	static const int		cfgBenchmark7_Reference		=	6307;
+	//[8] insert/remove
+	bool					cfgBenchmark8_Enable		=	cfgEnable;
+	static const int		cfgBenchmark8_Passes		=	32;
+	static const int		cfgBenchmark8_Iterations	=	65536;
+	static const int		cfgBenchmark8_Reference		=	2105;
+	//[9] updates (teleport)
+	bool					cfgBenchmark9_Enable		=	cfgEnable;
+	static const int		cfgBenchmark9_Passes		=	32;
+	static const int		cfgBenchmark9_Iterations	=	65536;
+	static const int		cfgBenchmark9_Reference		=	1879;
+	//[10] updates (jitter)
+	bool					cfgBenchmark10_Enable		=	cfgEnable;
+	static const btScalar	cfgBenchmark10_Scale		=	cfgVolumeCenterScale/10000;
+	static const int		cfgBenchmark10_Passes		=	32;
+	static const int		cfgBenchmark10_Iterations	=	65536;
+	static const int		cfgBenchmark10_Reference	=	1244;
+	//[11] optimize (incremental)
+	bool					cfgBenchmark11_Enable		=	cfgEnable;
+	static const int		cfgBenchmark11_Passes		=	64;
+	static const int		cfgBenchmark11_Iterations	=	65536;
+	static const int		cfgBenchmark11_Reference	=	2510;
+	//[12] btDbvtVolume notequal
+	bool					cfgBenchmark12_Enable		=	cfgEnable;
+	static const int		cfgBenchmark12_Iterations	=	32;
+	static const int		cfgBenchmark12_Reference	=	3677;
+	//[13] culling(OCL+fullsort)
+	bool					cfgBenchmark13_Enable		=	cfgEnable;
+	static const int		cfgBenchmark13_Iterations	=	1024;
+	static const int		cfgBenchmark13_Reference	=	2231;
+	//[14] culling(OCL+qsort)
+	bool					cfgBenchmark14_Enable		=	cfgEnable;
+	static const int		cfgBenchmark14_Iterations	=	8192;
+	static const int		cfgBenchmark14_Reference	=	3500;
+	//[15] culling(KDOP+qsort)
+	bool					cfgBenchmark15_Enable		=	cfgEnable;
+	static const int		cfgBenchmark15_Iterations	=	8192;
+	static const int		cfgBenchmark15_Reference	=	1151;
+	//[16] insert/remove batch
+	bool					cfgBenchmark16_Enable		=	cfgEnable;
+	static const int		cfgBenchmark16_BatchCount	=	256;
+	static const int		cfgBenchmark16_Passes		=	16384;
+	static const int		cfgBenchmark16_Reference	=	5138;
+	//[17] select
+	bool					cfgBenchmark17_Enable		=	cfgEnable;
+	static const int		cfgBenchmark17_Iterations	=	4;
+	static const int		cfgBenchmark17_Reference	=	3390;
+
+	btClock					wallclock;
+	printf("Benchmarking dbvt...\r\n");
+	printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
+	printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
+	printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
+	printf("\tLeaves: %u\r\n",cfgLeaves);
+	printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
+	printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
+	if(cfgBenchmark1_Enable)
+	{// Benchmark 1	
+		srand(380843);
+		btAlignedObjectArray<btDbvtVolume>	volumes;
+		btAlignedObjectArray<bool>			results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[1] btDbvtVolume intersections: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark1_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					results[k]=Intersect(volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
+	}
+	if(cfgBenchmark2_Enable)
+	{// Benchmark 2	
+		srand(380843);
+		btAlignedObjectArray<btDbvtVolume>	volumes;
+		btAlignedObjectArray<btDbvtVolume>	results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[2] btDbvtVolume merges: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark2_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					Merge(volumes[j],volumes[k],results[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
+	}
+	if(cfgBenchmark3_Enable)
+	{// Benchmark 3	
+		srand(380843);
+		btDbvt						dbvt[2];
+		btDbvtBenchmark::NilPolicy	policy;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[3] btDbvt::collideTT: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark3_Iterations;++i)
+		{
+			btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
+	}
+	if(cfgBenchmark4_Enable)
+	{// Benchmark 4
+		srand(380843);
+		btDbvt						dbvt;
+		btDbvtBenchmark::NilPolicy	policy;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[4] btDbvt::collideTT self: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark4_Iterations;++i)
+		{
+			btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
+	}
+	if(cfgBenchmark5_Enable)
+	{// Benchmark 5	
+		srand(380843);
+		btDbvt								dbvt[2];
+		btAlignedObjectArray<btTransform>	transforms;
+		btDbvtBenchmark::NilPolicy			policy;
+		transforms.resize(cfgBenchmark5_Iterations);
+		for(int i=0;i<transforms.size();++i)
+		{
+			transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[5] btDbvt::collideTT xform: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark5_Iterations;++i)
+		{
+			btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
+	}
+	if(cfgBenchmark6_Enable)
+	{// Benchmark 6	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btTransform>	transforms;
+		btDbvtBenchmark::NilPolicy			policy;
+		transforms.resize(cfgBenchmark6_Iterations);
+		for(int i=0;i<transforms.size();++i)
+		{
+			transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[6] btDbvt::collideTT xform,self: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark6_Iterations;++i)
+		{
+			btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);		
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
+	}
+	if(cfgBenchmark7_Enable)
+	{// Benchmark 7	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btVector3>		rayorg;
+		btAlignedObjectArray<btVector3>		raydir;
+		btDbvtBenchmark::NilPolicy			policy;
+		rayorg.resize(cfgBenchmark7_Iterations);
+		raydir.resize(cfgBenchmark7_Iterations);
+		for(int i=0;i<rayorg.size();++i)
+		{
+			rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+			raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[7] btDbvt::rayTest: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark7_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark7_Iterations;++j)
+			{
+				btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		unsigned	rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
+		printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
+	}
+	if(cfgBenchmark8_Enable)
+	{// Benchmark 8	
+		srand(380843);
+		btDbvt								dbvt;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[8] insert/remove: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark8_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark8_Iterations;++j)
+			{
+				dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
+		printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
+	}
+	if(cfgBenchmark9_Enable)
+	{// Benchmark 9	
+		srand(380843);
+		btDbvt										dbvt;
+		btAlignedObjectArray<const btDbvtNode*>	leaves;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		dbvt.extractLeaves(dbvt.m_root,leaves);
+		printf("[9] updates (teleport): ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark9_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark9_Iterations;++j)
+			{
+				dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
+					btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
+	}
+	if(cfgBenchmark10_Enable)
+	{// Benchmark 10	
+		srand(380843);
+		btDbvt										dbvt;
+		btAlignedObjectArray<const btDbvtNode*>	leaves;
+		btAlignedObjectArray<btVector3>				vectors;
+		vectors.resize(cfgBenchmark10_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		dbvt.extractLeaves(dbvt.m_root,leaves);
+		printf("[10] updates (jitter): ");
+		wallclock.reset();
+
+		for(int i=0;i<cfgBenchmark10_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark10_Iterations;++j)
+			{			
+				const btVector3&	d=vectors[j];
+				btDbvtNode*		l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
+				btDbvtVolume		v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
+				dbvt.update(l,v);
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
+	}
+	if(cfgBenchmark11_Enable)
+	{// Benchmark 11	
+		srand(380843);
+		btDbvt										dbvt;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[11] optimize (incremental): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark11_Passes;++i)
+		{
+			dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
+		printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
+	}
+	if(cfgBenchmark12_Enable)
+	{// Benchmark 12	
+		srand(380843);
+		btAlignedObjectArray<btDbvtVolume>	volumes;
+		btAlignedObjectArray<bool>				results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[12] btDbvtVolume notequal: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark12_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					results[k]=NotEqual(volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
+	}
+	if(cfgBenchmark13_Enable)
+	{// Benchmark 13	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btVector3>		vectors;
+		btDbvtBenchmark::NilPolicy			policy;
+		vectors.resize(cfgBenchmark13_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[13] culling(OCL+fullsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark13_Iterations;++i)
+		{
+			static const btScalar	offset=0;
+			policy.m_depth=-SIMD_INFINITY;
+			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark13_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark14_Enable)
+	{// Benchmark 14	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btVector3>		vectors;
+		btDbvtBenchmark::P14				policy;
+		vectors.resize(cfgBenchmark14_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		policy.m_nodes.reserve(cfgLeaves);
+		printf("[14] culling(OCL+qsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark14_Iterations;++i)
+		{
+			static const btScalar	offset=0;
+			policy.m_nodes.resize(0);
+			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
+			policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark14_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark15_Enable)
+	{// Benchmark 15	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btVector3>		vectors;
+		btDbvtBenchmark::P15				policy;
+		vectors.resize(cfgBenchmark15_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
+		}
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		policy.m_nodes.reserve(cfgLeaves);
+		printf("[15] culling(KDOP+qsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark15_Iterations;++i)
+		{
+			static const btScalar	offset=0;
+			policy.m_nodes.resize(0);
+			policy.m_axis=vectors[i];
+			dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
+			policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark15_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark16_Enable)
+	{// Benchmark 16	
+		srand(380843);
+		btDbvt								dbvt;
+		btAlignedObjectArray<btDbvtNode*>	batch;
+		btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		batch.reserve(cfgBenchmark16_BatchCount);
+		printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark16_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			{
+				batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+			}
+			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			{
+				dbvt.remove(batch[j]);
+			}
+			batch.resize(0);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
+		printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
+	}
+	if(cfgBenchmark17_Enable)
+	{// Benchmark 17
+		srand(380843);
+		btAlignedObjectArray<btDbvtVolume>	volumes;
+		btAlignedObjectArray<int>			results;
+		btAlignedObjectArray<int>			indices;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		indices.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			indices[i]=i;
+			volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			btSwap(indices[i],indices[rand()%cfgLeaves]);
+		}
+		printf("[17] btDbvtVolume select: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark17_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					const int idx=indices[k];
+					results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
+	}
+	printf("\r\n\r\n");
+}
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h	(revision 1963)
@@ -0,0 +1,1106 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvt implementation by Nathanael Presson
+
+#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAabbUtil2.h"
+
+//
+// Compile time configuration
+//
+
+
+// Implementation profiles
+#define DBVT_IMPL_GENERIC		0	// Generic implementation	
+#define DBVT_IMPL_SSE			1	// SSE
+
+// 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
+#else
+#define	DBVT_USE_TEMPLATE		0
+#endif
+
+// Use only intrinsics instead of inline asm
+#define DBVT_USE_INTRINSIC_SSE	1
+
+// Using memmov for collideOCL
+#define DBVT_USE_MEMMOVE		1
+
+// Enable benchmarking code
+#define	DBVT_ENABLE_BENCHMARK	0
+
+// Inlining
+#define DBVT_INLINE				SIMD_FORCE_INLINE
+// Align
+#ifdef WIN32
+#define DBVT_ALIGN				__declspec(align(16))
+#else
+#define DBVT_ALIGN
+#endif
+
+// Specific methods implementation
+
+//SSE gives errors on a MSVC 7.1
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#define DBVT_SELECT_IMPL		DBVT_IMPL_SSE
+#define DBVT_MERGE_IMPL			DBVT_IMPL_SSE
+#define DBVT_INT0_IMPL			DBVT_IMPL_SSE
+#else
+#define DBVT_SELECT_IMPL		DBVT_IMPL_GENERIC
+#define DBVT_MERGE_IMPL			DBVT_IMPL_GENERIC
+#define DBVT_INT0_IMPL			DBVT_IMPL_GENERIC
+#endif
+
+#if	(DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||	\
+	(DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||	\
+	(DBVT_INT0_IMPL==DBVT_IMPL_SSE)
+#include <emmintrin.h>
+#endif
+
+//
+// Auto config and checks
+//
+
+#if DBVT_USE_TEMPLATE
+#define	DBVT_VIRTUAL
+#define DBVT_VIRTUAL_DTOR(a)
+#define DBVT_PREFIX					template <typename T>
+#define DBVT_IPOLICY				T& policy
+#define DBVT_CHECKTYPE				static const ICollide&	typechecker=*(T*)0;
+#else
+#define	DBVT_VIRTUAL_DTOR(a)		virtual ~a() {}
+#define DBVT_VIRTUAL				virtual
+#define DBVT_PREFIX
+#define DBVT_IPOLICY				ICollide& policy
+#define DBVT_CHECKTYPE
+#endif
+
+#if DBVT_USE_MEMMOVE
+#ifndef __CELLOS_LV2__
+#include <memory.h>
+#endif
+#include <string.h>
+#endif
+
+#ifndef DBVT_USE_TEMPLATE
+#error "DBVT_USE_TEMPLATE undefined"
+#endif
+
+#ifndef DBVT_USE_MEMMOVE
+#error "DBVT_USE_MEMMOVE undefined"
+#endif
+
+#ifndef DBVT_ENABLE_BENCHMARK
+#error "DBVT_ENABLE_BENCHMARK undefined"
+#endif
+
+#ifndef DBVT_SELECT_IMPL
+#error "DBVT_SELECT_IMPL undefined"
+#endif
+
+#ifndef DBVT_MERGE_IMPL
+#error "DBVT_MERGE_IMPL undefined"
+#endif
+
+#ifndef DBVT_INT0_IMPL
+#error "DBVT_INT0_IMPL undefined"
+#endif
+
+//
+// Defaults volumes
+//
+
+/* btDbvtAabbMm			*/ 
+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 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;
+private:
+	btVector3	mi,mx;
+};
+
+// Types	
+typedef	btDbvtAabbMm	btDbvtVolume;
+
+/* btDbvtNode				*/ 
+struct	btDbvtNode
+{
+	btDbvtVolume	volume;
+	btDbvtNode*		parent;
+	DBVT_INLINE bool	isleaf() const		{ return(childs[1]==0); }
+	DBVT_INLINE bool	isinternal() const	{ return(!isleaf()); }
+	union	{
+		btDbvtNode*	childs[2];
+		void*	data;
+		int		dataAsInt;
+	};
+};
+
+///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
+///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;
+		btScalar	value;
+		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*)					{}
+		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
+	};
+
+	// Fields
+	btDbvtNode*		m_root;
+	btDbvtNode*		m_free;
+	int				m_lkhd;
+	int				m_leaves;
+	unsigned		m_opath;
+	// Methods
+	btDbvt();
+	~btDbvt();
+	void			clear();
+	bool			empty() const { return(0==m_root); }
+	void			optimizeBottomUp();
+	void			optimizeTopDown(int bu_treshold=128);
+	void			optimizeIncremental(int passes);
+	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			remove(btDbvtNode* leaf);
+	void			write(IWriter* iwriter) const;
+	void			clone(btDbvt& dest,IClone* iclone=0) const;
+	static int		maxdepth(const btDbvtNode* node);
+	static int		countLeaves(const btDbvtNode* node);
+	static void		extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
+#if DBVT_ENABLE_BENCHMARK
+	static void		benchmark();
+#else
+	static void		benchmark(){}
+#endif
+	// DBVT_IPOLICY must support ICollide policy/interface
+	DBVT_PREFIX
+		static void		enumNodes(	const btDbvtNode* root,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		static void		enumLeaves(	const btDbvtNode* root,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		static 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);
+	DBVT_PREFIX
+		static void		collideTT(	const btDbvtNode* root0,
+		const btTransform& xform0,
+		const btDbvtNode* root1,
+		const btTransform& xform1,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		static void		collideTV(	const btDbvtNode* root,
+		const btDbvtVolume& volume,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		static void		rayTest(	const btDbvtNode* root,
+		const btVector3& rayFrom,
+		const btVector3& rayTo,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		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)
+	{
+		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); }
+		return(i); 
+	}
+	//
+private:
+	btDbvt(const btDbvt&)	{}	
+};
+
+//
+// Inline's
+//
+
+//
+inline btDbvtAabbMm			btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
+{
+	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)));
+}
+
+//
+inline btDbvtAabbMm			btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
+{
+	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]);
+	}
+	return(box);
+}
+
+//
+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]);
+	}
+	return(box);
+}
+
+//
+DBVT_INLINE void		btDbvtAabbMm::Expand(const btVector3& 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]);
+}
+
+//
+DBVT_INLINE bool		btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
+{
+	return(	(mi.x()<=a.mi.x())&&
+		(mi.y()<=a.mi.y())&&
+		(mi.z()<=a.mi.z())&&
+		(mx.x()>=a.mx.x())&&
+		(mx.y()>=a.mx.y())&&
+		(mx.z()>=a.mx.z()));
+}
+
+//
+DBVT_INLINE int		btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
+{
+	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;
+	case	(1+0+0):	px=btVector3(mx.x(),mi.y(),mi.z());
+		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;
+	case	(1+2+0):	px=btVector3(mx.x(),mx.y(),mi.z());
+		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;
+	case	(1+0+4):	px=btVector3(mx.x(),mi.y(),mx.z());
+		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;
+	case	(1+2+4):	px=btVector3(mx.x(),mx.y(),mx.z());
+		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);
+}
+
+//
+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));
+}
+
+//
+DBVT_INLINE void		btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
+{
+	for(int i=0;i<3;++i)
+	{
+		if(d[i]<0)
+		{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
+		else
+		{ smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
+	}
+}
+
+//
+DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
+								  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);
+#else
+	return(	(a.mi.x()<=b.mx.x())&&
+		(a.mx.x()>=b.mi.x())&&
+		(a.mi.y()<=b.mx.y())&&
+		(a.mx.y()>=b.mi.y())&&
+		(a.mi.z()<=b.mx.z())&&		
+		(a.mx.z()>=b.mi.z()));
+#endif
+}
+
+//
+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);
+}
+
+//
+DBVT_INLINE bool		Intersect(	const btDbvtAabbMm& a,
+								  const btVector3& b)
+{
+	return(	(b.x()>=a.mi.x())&&
+		(b.y()>=a.mi.y())&&
+		(b.z()>=a.mi.z())&&
+		(b.x()<=a.mx.x())&&
+		(b.y()<=a.mx.y())&&
+		(b.z()<=a.mx.z()));
+}
+
+
+
+
+
+//////////////////////////////////////
+
+
+//
+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()));
+}
+
+//
+DBVT_INLINE int			Select(	const btDbvtAabbMm& o,
+							   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
+	__m128	omi(_mm_load_ps(o.mi));
+	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
+	__m128	ami(_mm_load_ps(a.mi));
+	ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
+	ami=_mm_sub_ps(ami,omi);
+	ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
+	__m128	bmi(_mm_load_ps(b.mi));
+	bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
+	bmi=_mm_sub_ps(bmi,omi);
+	bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
+	__m128	t0(_mm_movehl_ps(ami,ami));
+	ami=_mm_add_ps(ami,t0);
+	ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
+	__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];
+	__asm
+	{
+		mov		eax,o
+			mov		ecx,a
+			mov		edx,b
+			movaps	xmm0,[eax]
+		movaps	xmm5,mask
+			addps	xmm0,[eax+16]	
+		movaps	xmm1,[ecx]
+		movaps	xmm2,[edx]
+		addps	xmm1,[ecx+16]
+		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
+	}
+	return(r[0]&1);
+#endif
+#else
+	return(Proximity(o,a)<Proximity(o,b)?0:1);
+#endif
+}
+
+//
+DBVT_INLINE void		Merge(	const btDbvtAabbMm& a,
+							  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);
+#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];
+	}
+#endif
+}
+
+//
+DBVT_INLINE bool		NotEqual(	const btDbvtAabbMm& a,
+								 const btDbvtAabbMm& b)
+{
+	return(	(a.mi.x()!=b.mi.x())||
+		(a.mi.y()!=b.mi.y())||
+		(a.mi.z()!=b.mi.z())||
+		(a.mx.x()!=b.mx.x())||
+		(a.mx.y()!=b.mx.y())||
+		(a.mx.z()!=b.mx.z()));
+}
+
+//
+// Inline's
+//
+
+//
+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_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_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)
+				{
+					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);
+						}
+					}
+					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 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]);
+						}
+						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::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};
+
+
+			btVector3 resultNormal;
+
+
+			btAlignedObjectArray<const btDbvtNode*>	stack;
+			stack.reserve(SIMPLE_STACKSIZE);
+			stack.push_back(root);
+			do	{
+				const btDbvtNode*	node=stack[stack.size()-1];
+				stack.pop_back();
+
+				btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()};
+				btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
+				btScalar tmin=1.f,lambda_min=0.f;
+				bool 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())
+					{
+						stack.push_back(node->childs[0]);
+						stack.push_back(node->childs[1]);
+					}
+					else
+					{
+						policy.Process(node);
+					}
+				}
+			} while(stack.size());
+		}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideKDOP(const btDbvtNode* root,
+									const btVector3* normals,
+									const btScalar* offsets,
+									int count,
+									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)
+			{
+				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());
+		}
+}
+
+//
+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)
+			{
+				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());
+		}
+}
+
+//
+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);
+		}
+}
+
+//
+// PP Cleanup
+//
+
+#undef DBVT_USE_MEMMOVE
+#undef DBVT_USE_TEMPLATE
+#undef DBVT_VIRTUAL_DTOR
+#undef DBVT_VIRTUAL
+#undef DBVT_PREFIX
+#undef DBVT_IPOLICY
+#undef DBVT_CHECKTYPE
+#undef DBVT_IMPL_GENERIC
+#undef DBVT_IMPL_SSE
+#undef DBVT_USE_INTRINSIC_SSE
+#undef DBVT_SELECT_IMPL
+#undef DBVT_MERGE_IMPL
+#undef DBVT_INT0_IMPL
+
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp	(revision 1963)
@@ -0,0 +1,592 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvtBroadphase implementation by Nathanael Presson
+
+#include "btDbvtBroadphase.h"
+
+//
+// Profiling
+//
+
+#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK
+#include <stdio.h>
+#endif
+
+#if DBVT_BP_PROFILE
+struct	ProfileScope
+{
+	__forceinline ProfileScope(btClock& clock,unsigned long& value) :
+	m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
+	{
+	}
+	__forceinline ~ProfileScope()
+	{
+		(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
+	}
+	btClock*		m_clock;
+	unsigned long*	m_value;
+	unsigned long	m_base;
+};
+#define	SPC(_value_)	ProfileScope	spc_scope(m_clock,_value_)
+#else
+#define	SPC(_value_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void	listappend(T* item,T*& list)
+{
+	item->links[0]=0;
+	item->links[1]=list;
+	if(list) list->links[0]=item;
+	list=item;
+}
+
+//
+template <typename T>
+static inline void	listremove(T* item,T*& list)
+{
+	if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
+	if(item->links[1]) item->links[1]->links[0]=item->links[0];
+}
+
+//
+template <typename T>
+static inline int	listcount(T* root)
+{
+	int	n=0;
+	while(root) { ++n;root=root->links[1]; }
+	return(n);
+}
+
+//
+template <typename T>
+static inline void	clear(T& value)
+{
+	static const struct ZeroDummy : T {} zerodummy;
+	value=zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider	*/ 
+struct	btDbvtTreeCollider : btDbvt::ICollide
+{
+	btDbvtBroadphase*	pbp;
+	btDbvtProxy*		proxy;
+	btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
+	void	Process(const btDbvtNode* na,const btDbvtNode* nb)
+	{
+		if(na!=nb)
+		{
+			btDbvtProxy*	pa=(btDbvtProxy*)na->data;
+			btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
+#if DBVT_BP_SORTPAIRS
+			if(pa>pb) btSwap(pa,pb);
+#endif
+			pbp->m_paircache->addOverlappingPair(pa,pb);
+			++pbp->m_newpairs;
+		}
+	}
+	void	Process(const btDbvtNode* n)
+	{
+		Process(n,proxy->leaf);
+	}
+};
+
+//
+// btDbvtBroadphase
+//
+
+//
+btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
+{
+	m_deferedcollide	=	false;
+	m_needcleanup		=	true;
+	m_releasepaircache	=	(paircache!=0)?false:true;
+	m_prediction		=	1/(btScalar)2;
+	m_stageCurrent		=	0;
+	m_fixedleft			=	0;
+	m_fupdates			=	1;
+	m_dupdates			=	0;
+	m_cupdates			=	10;
+	m_newpairs			=	1;
+	m_updates_call		=	0;
+	m_updates_done		=	0;
+	m_updates_ratio		=	0;
+	m_paircache			=	paircache?
+paircache	:
+	new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+	m_gid				=	0;
+	m_pid				=	0;
+	m_cid				=	0;
+	for(int i=0;i<=STAGECOUNT;++i)
+	{
+		m_stageRoots[i]=0;
+	}
+#if DBVT_BP_PROFILE
+	clear(m_profiling);
+#endif
+}
+
+//
+btDbvtBroadphase::~btDbvtBroadphase()
+{
+	if(m_releasepaircache) 
+	{
+		m_paircache->~btOverlappingPairCache();
+		btAlignedFree(m_paircache);
+	}
+}
+
+//
+btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
+															  const btVector3& aabbMax,
+															  int /*shapeType*/,
+															  void* userPtr,
+															  short int collisionFilterGroup,
+															  short int collisionFilterMask,
+															  btDispatcher* /*dispatcher*/,
+															  void* /*multiSapProxy*/)
+{
+	btDbvtProxy*		proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	aabbMin,aabbMax,userPtr,
+		collisionFilterGroup,
+		collisionFilterMask);
+
+	btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
+
+	//bproxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
+	proxy->stage		=	m_stageCurrent;
+	proxy->m_uniqueId	=	++m_gid;
+	proxy->leaf			=	m_sets[0].insert(aabb,proxy);
+	listappend(proxy,m_stageRoots[m_stageCurrent]);
+	if(!m_deferedcollide)
+	{
+		btDbvtTreeCollider	collider(this);
+		collider.proxy=proxy;
+		btDbvt::collideTV(m_sets[0].m_root,aabb,collider);
+		btDbvt::collideTV(m_sets[1].m_root,aabb,collider);
+	}
+	return(proxy);
+}
+
+//
+void							btDbvtBroadphase::destroyProxy(	btBroadphaseProxy* absproxy,
+															   btDispatcher* dispatcher)
+{
+	btDbvtProxy*	proxy=(btDbvtProxy*)absproxy;
+	if(proxy->stage==STAGECOUNT)
+		m_sets[1].remove(proxy->leaf);
+	else
+		m_sets[0].remove(proxy->leaf);
+	listremove(proxy,m_stageRoots[proxy->stage]);
+	m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
+	btAlignedFree(proxy);
+	m_needcleanup=true;
+}
+
+void	btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+
+void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+
+	struct	BroadphaseRayTester : btDbvt::ICollide
+	{
+		btBroadphaseRayCallback& m_rayCallback;
+		BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
+			:m_rayCallback(orgCallback)
+		{
+		}
+		void					Process(const btDbvtNode* leaf)
+		{
+			btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+			m_rayCallback.process(proxy);
+		}
+	};	
+
+	BroadphaseRayTester callback(rayCallback);
+
+	m_sets[0].rayTest(	m_sets[0].m_root,
+		rayFrom,
+		rayTo,
+		callback);
+
+	m_sets[1].rayTest(	m_sets[1].m_root,
+		rayFrom,
+		rayTo,
+		callback);
+
+}
+
+//
+void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
+														  const btVector3& aabbMin,
+														  const btVector3& aabbMax,
+														  btDispatcher* /*dispatcher*/)
+{
+	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+#if DBVT_BP_PREVENTFALSEUPDATE
+	if(NotEqual(aabb,proxy->leaf->volume))
+#endif
+	{
+		bool	docollide=false;
+		if(proxy->stage==STAGECOUNT)
+		{/* fixed -> dynamic set	*/ 
+			m_sets[1].remove(proxy->leaf);
+			proxy->leaf=m_sets[0].insert(aabb,proxy);
+			docollide=true;
+		}
+		else
+		{/* dynamic set				*/ 
+			++m_updates_call;
+			if(Intersect(proxy->leaf->volume,aabb))
+			{/* Moving				*/ 
+
+				const btVector3	delta=aabbMin-proxy->m_aabbMin;
+				btVector3		velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
+				if(delta[0]<0) velocity[0]=-velocity[0];
+				if(delta[1]<0) velocity[1]=-velocity[1];
+				if(delta[2]<0) velocity[2]=-velocity[2];
+				if	(
+#ifdef DBVT_BP_MARGIN				
+					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
+#else
+					m_sets[0].update(proxy->leaf,aabb,velocity)
+#endif
+					)
+				{
+					++m_updates_done;
+					docollide=true;
+				}
+			}
+			else
+			{/* Teleporting			*/ 
+				m_sets[0].update(proxy->leaf,aabb);
+				++m_updates_done;
+				docollide=true;
+			}	
+		}
+		listremove(proxy,m_stageRoots[proxy->stage]);
+		proxy->m_aabbMin = aabbMin;
+		proxy->m_aabbMax = aabbMax;
+		proxy->stage	=	m_stageCurrent;
+		listappend(proxy,m_stageRoots[m_stageCurrent]);
+		if(docollide)
+		{
+			m_needcleanup=true;
+			if(!m_deferedcollide)
+			{
+				btDbvtTreeCollider	collider(this);
+				btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider);
+				btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider);
+			}
+		}	
+	}
+}
+
+//
+void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+	collide(dispatcher);
+#if DBVT_BP_PROFILE
+	if(0==(m_pid%DBVT_BP_PROFILING_RATE))
+	{	
+		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
+		unsigned int	total=m_profiling.m_total;
+		if(total<=0) total=1;
+		printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
+		printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE);
+		printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE);
+		printf("total:     %uus\r\n",total/DBVT_BP_PROFILING_RATE);
+		const unsigned long	sum=m_profiling.m_ddcollide+
+			m_profiling.m_fdcollide+
+			m_profiling.m_cleanup;
+		printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
+		printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE));
+		clear(m_profiling);
+		m_clock.reset();
+	}
+#endif
+}
+
+//
+void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
+{
+	SPC(m_profiling.m_total);
+	/* optimize				*/ 
+	m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
+	if(m_fixedleft)
+	{
+		const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
+		m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
+		m_fixedleft=btMax<int>(0,m_fixedleft-count);
+	}
+	/* dynamic -> fixed set	*/ 
+	m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
+	btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
+	if(current)
+	{
+		btDbvtTreeCollider	collider(this);
+		do	{
+			btDbvtProxy*	next=current->links[1];
+			listremove(current,m_stageRoots[current->stage]);
+			listappend(current,m_stageRoots[STAGECOUNT]);
+#if DBVT_BP_ACCURATESLEEPING
+			m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
+			collider.proxy=current;
+			btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider);
+			btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider);
+#endif
+			m_sets[0].remove(current->leaf);
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)	curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
+			current->leaf	=	m_sets[1].insert(curAabb,current);
+			current->stage	=	STAGECOUNT;	
+			current			=	next;
+		} while(current);
+		m_fixedleft=m_sets[1].m_leaves;
+		m_needcleanup=true;
+	}
+	/* collide dynamics		*/ 
+	{
+		btDbvtTreeCollider	collider(this);
+		if(m_deferedcollide)
+		{
+			SPC(m_profiling.m_fdcollide);
+			btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
+		}
+		if(m_deferedcollide)
+		{
+			SPC(m_profiling.m_ddcollide);
+			btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
+		}
+	}
+	/* clean up				*/ 
+	if(m_needcleanup)
+	{
+		SPC(m_profiling.m_cleanup);
+		btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
+		if(pairs.size()>0)
+		{
+			const int	ci=pairs.size();
+			int			ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+			for(int i=0;i<ni;++i)
+			{
+				btBroadphasePair&	p=pairs[(m_cid+i)%ci];
+				btDbvtProxy*		pa=(btDbvtProxy*)p.m_pProxy0;
+				btDbvtProxy*		pb=(btDbvtProxy*)p.m_pProxy1;
+				if(!Intersect(pa->leaf->volume,pb->leaf->volume))
+				{
+#if DBVT_BP_SORTPAIRS
+					if(pa>pb) btSwap(pa,pb);
+#endif
+					m_paircache->removeOverlappingPair(pa,pb,dispatcher);
+					--ni;--i;
+				}
+			}
+			if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
+		}
+	}
+	++m_pid;
+	m_newpairs=1;
+	m_needcleanup=false;
+	if(m_updates_call>0)
+	{ m_updates_ratio=m_updates_done/(btScalar)m_updates_call; }
+	else
+	{ m_updates_ratio=0; }
+	m_updates_done/=2;
+	m_updates_call/=2;
+}
+
+//
+void							btDbvtBroadphase::optimize()
+{
+	m_sets[0].optimizeTopDown();
+	m_sets[1].optimizeTopDown();
+}
+
+//
+btOverlappingPairCache*			btDbvtBroadphase::getOverlappingPairCache()
+{
+	return(m_paircache);
+}
+
+//
+const btOverlappingPairCache*	btDbvtBroadphase::getOverlappingPairCache() const
+{
+	return(m_paircache);
+}
+
+//
+void							btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds;
+
+	if(!m_sets[0].empty())
+		if(!m_sets[1].empty())	Merge(	m_sets[0].m_root->volume,
+			m_sets[1].m_root->volume,bounds);
+		else
+			bounds=m_sets[0].m_root->volume;
+	else if(!m_sets[1].empty())	bounds=m_sets[1].m_root->volume;
+	else
+		bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0);
+	aabbMin=bounds.Mins();
+	aabbMax=bounds.Maxs();
+}
+
+//
+void							btDbvtBroadphase::printStats()
+{}
+
+//
+#if DBVT_BP_ENABLE_BENCHMARK
+
+struct	btBroadphaseBenchmark
+{
+	struct	Experiment
+	{
+		const char*			name;
+		int					object_count;
+		int					update_count;
+		int					spawn_count;
+		int					iterations;
+		btScalar			speed;
+		btScalar			amplitude;
+	};
+	struct	Object
+	{
+		btVector3			center;
+		btVector3			extents;
+		btBroadphaseProxy*	proxy;
+		btScalar			time;
+		void				update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi)
+		{
+			time		+=	speed;
+			center[0]	=	btCos(time*(btScalar)2.17)*amplitude+
+				btSin(time)*amplitude/2;
+			center[1]	=	btCos(time*(btScalar)1.38)*amplitude+
+				btSin(time)*amplitude;
+			center[2]	=	btSin(time*(btScalar)0.777)*amplitude;
+			pbi->setAabb(proxy,center-extents,center+extents,0);
+		}
+	};
+	static int		UnsignedRand(int range=RAND_MAX-1)	{ return(rand()%(range+1)); }
+	static btScalar	UnitRand()							{ return(UnsignedRand(16384)/(btScalar)16384); }
+	static void		OutputTime(const char* name,btClock& c,unsigned count=0)
+	{
+		const unsigned long	us=c.getTimeMicroseconds();
+		const unsigned long	ms=(us+500)/1000;
+		const btScalar		sec=us/(btScalar)(1000*1000);
+		if(count>0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
+		else
+			printf("%s : %u us (%u ms)\r\n",name,us,ms);
+	}
+};
+
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi)
+{
+	static const btBroadphaseBenchmark::Experiment		experiments[]=
+	{
+		{"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100},
+		/*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100},
+		{"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/
+	};
+	static const int										nexperiments=sizeof(experiments)/sizeof(experiments[0]);
+	btAlignedObjectArray<btBroadphaseBenchmark::Object*>	objects;
+	btClock													wallclock;
+	/* Begin			*/ 
+	for(int iexp=0;iexp<nexperiments;++iexp)
+	{
+		const btBroadphaseBenchmark::Experiment&	experiment=experiments[iexp];
+		const int									object_count=experiment.object_count;
+		const int									update_count=(object_count*experiment.update_count)/100;
+		const int									spawn_count=(object_count*experiment.spawn_count)/100;
+		const btScalar								speed=experiment.speed;	
+		const btScalar								amplitude=experiment.amplitude;
+		printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
+		printf("\tObjects: %u\r\n",object_count);
+		printf("\tUpdate: %u\r\n",update_count);
+		printf("\tSpawn: %u\r\n",spawn_count);
+		printf("\tSpeed: %f\r\n",speed);
+		printf("\tAmplitude: %f\r\n",amplitude);
+		srand(180673);
+		/* Create objects	*/ 
+		wallclock.reset();
+		objects.reserve(object_count);
+		for(int i=0;i<object_count;++i)
+		{
+			btBroadphaseBenchmark::Object*	po=new btBroadphaseBenchmark::Object();
+			po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
+			po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
+			po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
+			po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
+			po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
+			po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
+			po->time=btBroadphaseBenchmark::UnitRand()*2000;
+			po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
+			objects.push_back(po);
+		}
+		btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
+		/* First update		*/ 
+		wallclock.reset();
+		for(int i=0;i<objects.size();++i)
+		{
+			objects[i]->update(speed,amplitude,pbi);
+		}
+		btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
+		/* Updates			*/ 
+		wallclock.reset();
+		for(int i=0;i<experiment.iterations;++i)
+		{
+			for(int j=0;j<update_count;++j)
+			{				
+				objects[j]->update(speed,amplitude,pbi);
+			}
+			pbi->calculateOverlappingPairs(0);
+		}
+		btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
+		/* Clean up			*/ 
+		wallclock.reset();
+		for(int i=0;i<objects.size();++i)
+		{
+			pbi->destroyProxy(objects[i]->proxy,0);
+			delete objects[i];
+		}
+		objects.resize(0);
+		btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+	}
+
+}
+#else
+void							btDbvtBroadphase::benchmark(btBroadphaseInterface*)
+{}
+#endif
+
+#if DBVT_BP_PROFILE
+#undef	SPC
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h	(revision 1963)
@@ -0,0 +1,119 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btDbvtBroadphase implementation by Nathanael Presson
+#ifndef BT_DBVT_BROADPHASE_H
+#define BT_DBVT_BROADPHASE_H
+
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+
+//
+// Compile time config
+//
+
+#define	DBVT_BP_PROFILE					0
+#define DBVT_BP_SORTPAIRS				1
+#define DBVT_BP_PREVENTFALSEUPDATE		0
+#define DBVT_BP_ACCURATESLEEPING		0
+#define DBVT_BP_ENABLE_BENCHMARK		0
+#define DBVT_BP_MARGIN					(btScalar)0.05
+
+#if DBVT_BP_PROFILE
+#define	DBVT_BP_PROFILING_RATE	256
+#include "LinearMath/btQuickprof.h"
+#endif
+
+//
+// btDbvtProxy
+//
+struct btDbvtProxy : btBroadphaseProxy
+{
+	/* Fields		*/ 
+	//btDbvtAabbMm	aabb;
+	btDbvtNode*		leaf;
+	btDbvtProxy*	links[2];
+	int				stage;
+	/* ctor			*/ 
+	btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
+	btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
+	{
+		links[0]=links[1]=0;
+	}
+};
+
+typedef btAlignedObjectArray<btDbvtProxy*>	btDbvtProxyArray;
+
+///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
+///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
+///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
+struct	btDbvtBroadphase : btBroadphaseInterface
+{
+	/* Config		*/ 
+	enum	{
+		DYNAMIC_SET			=	0,	/* Dynamic set index	*/ 
+		FIXED_SET			=	1,	/* Fixed set index		*/ 
+		STAGECOUNT			=	2	/* Number of stages		*/ 
+	};
+	/* Fields		*/ 
+	btDbvt					m_sets[2];					// Dbvt sets
+	btDbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
+	btOverlappingPairCache*	m_paircache;				// Pair cache
+	btScalar				m_prediction;				// Velocity prediction
+	int						m_stageCurrent;				// Current stage
+	int						m_fupdates;					// % of fixed updates per frame
+	int						m_dupdates;					// % of dynamic updates per frame
+	int						m_cupdates;					// % of cleanup updates per frame
+	int						m_newpairs;					// Number of pairs created
+	int						m_fixedleft;				// Fixed optimization left
+	unsigned				m_updates_call;				// Number of updates call
+	unsigned				m_updates_done;				// Number of updates done
+	btScalar				m_updates_ratio;			// m_updates_done/m_updates_call
+	int						m_pid;						// Parse id
+	int						m_cid;						// Cleanup index
+	int						m_gid;						// Gen id
+	bool					m_releasepaircache;			// Release pair cache on delete
+	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
+	bool					m_needcleanup;				// Need to run cleanup?
+#if DBVT_BP_PROFILE
+	btClock					m_clock;
+	struct	{
+		unsigned long		m_total;
+		unsigned long		m_ddcollide;
+		unsigned long		m_fdcollide;
+		unsigned long		m_cleanup;
+		unsigned long		m_jobcount;
+	}				m_profiling;
+#endif
+	/* Methods		*/ 
+	btDbvtBroadphase(btOverlappingPairCache* paircache=0);
+	~btDbvtBroadphase();
+	void							collide(btDispatcher* dispatcher);
+	void							optimize();
+	/* btBroadphaseInterface Implementation	*/ 
+	btBroadphaseProxy*				createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
+	void							destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	void							setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	void							calculateOverlappingPairs(btDispatcher* dispatcher);
+	btOverlappingPairCache*			getOverlappingPairCache();
+	const btOverlappingPairCache*	getOverlappingPairCache() const;
+	void							getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+	void							printStats();
+	static void						benchmark(btBroadphaseInterface*);
+};
+
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.cpp	(revision 1963)
@@ -0,0 +1,22 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btDispatcher.h"
+
+btDispatcher::~btDispatcher()
+{
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h	(revision 1963)
@@ -0,0 +1,103 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _DISPATCHER_H
+#define _DISPATCHER_H
+
+#include "LinearMath/btScalar.h"
+
+class btCollisionAlgorithm;
+struct btBroadphaseProxy;
+class btRigidBody;
+class	btCollisionObject;
+class btOverlappingPairCache;
+
+
+class btPersistentManifold;
+class btStackAlloc;
+
+struct btDispatcherInfo
+{
+	enum DispatchFunc
+	{
+		DISPATCH_DISCRETE = 1,
+		DISPATCH_CONTINUOUS
+	};
+	btDispatcherInfo()
+		:m_timeStep(btScalar(0.)),
+		m_stepCount(0),
+		m_dispatchFunc(DISPATCH_DISCRETE),
+		m_timeOfImpact(btScalar(1.)),
+		m_useContinuous(false),
+		m_debugDraw(0),
+		m_enableSatConvex(false),
+		m_enableSPU(true),
+		m_useEpa(true),
+		m_allowedCcdPenetration(btScalar(0.04)),
+		m_stackAllocator(0)
+	{
+
+	}
+	btScalar	m_timeStep;
+	int		m_stepCount;
+	int		m_dispatchFunc;
+	mutable btScalar	m_timeOfImpact;
+	bool	m_useContinuous;
+	class btIDebugDraw*	m_debugDraw;
+	bool	m_enableSatConvex;
+	bool	m_enableSPU;
+	bool	m_useEpa;
+	btScalar	m_allowedCcdPenetration;
+	btStackAlloc*	m_stackAllocator;
+	
+};
+
+///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
+///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
+class btDispatcher
+{
+
+
+public:
+	virtual ~btDispatcher() ;
+
+	virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold=0) = 0;
+
+	virtual btPersistentManifold*	getNewManifold(void* body0,void* body1)=0;
+
+	virtual void releaseManifold(btPersistentManifold* manifold)=0;
+
+	virtual void clearManifold(btPersistentManifold* manifold)=0;
+
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1) = 0;
+
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0;
+
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)  =0;
+
+	virtual int getNumManifolds() const = 0;
+
+	virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0;
+
+	virtual	btPersistentManifold**	getInternalManifoldPointer() = 0;
+
+	virtual	void* allocateCollisionAlgorithm(int size)  = 0;
+
+	virtual	void freeCollisionAlgorithm(void* ptr) = 0;
+
+};
+
+
+#endif //_DISPATCHER_H
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp	(revision 1963)
@@ -0,0 +1,482 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMultiSapBroadphase.h"
+
+#include "btSimpleBroadphase.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "btQuantizedBvh.h"
+
+///	btSapBroadphaseArray	m_sapBroadphases;
+
+///	btOverlappingPairCache*	m_overlappingPairs;
+extern int gOverlappingPairs;
+
+/*
+class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
+{
+public:
+
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
+	}
+};
+
+*/
+
+btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
+:m_overlappingPairs(pairCache),
+m_optimizedAabbTree(0),
+m_ownsPairCache(false),
+m_invalidPair(0)
+{
+	if (!m_overlappingPairs)
+	{
+		m_ownsPairCache = true;
+		void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
+		m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
+	}
+
+	struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
+	{
+		virtual ~btMultiSapOverlapFilterCallback()
+		{}
+		// return true when pairs need collision
+		virtual bool	needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
+		{
+			btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
+			btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
+			
+			bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
+			collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
+	
+			return collides;
+		}
+	};
+
+	void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
+	m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
+
+	m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
+//	mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
+//	m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
+}
+
+btMultiSapBroadphase::~btMultiSapBroadphase()
+{
+	if (m_ownsPairCache)
+	{
+		m_overlappingPairs->~btOverlappingPairCache();
+		btAlignedFree(m_overlappingPairs);
+	}
+}
+
+
+void	btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
+{
+	m_optimizedAabbTree = new btQuantizedBvh();
+	m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
+	QuantizedNodeArray&	nodes = m_optimizedAabbTree->getLeafNodeArray();
+	for (int i=0;i<m_sapBroadphases.size();i++)
+	{
+		btQuantizedBvhNode node;
+		btVector3 aabbMin,aabbMax;
+		m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
+		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
+		m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+		int partId = 0;
+		node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
+		nodes.push_back(node);
+	}
+	m_optimizedAabbTree->buildInternal();
+}
+
+btBroadphaseProxy*	btMultiSapBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/)
+{
+	//void* ignoreMe -> we could think of recursive multi-sap, if someone is interested
+
+	void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
+	btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin,  aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
+	m_multiSapProxies.push_back(proxy);
+
+	///this should deal with inserting/removal into child broadphases
+	setAabb(proxy,aabbMin,aabbMax,dispatcher);
+	return proxy;
+}
+
+void	btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+{
+	///not yet
+	btAssert(0);
+
+}
+
+
+void	btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase)
+{
+	void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
+	btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
+	bridgeProxyRef->m_childProxy = childProxy;
+	bridgeProxyRef->m_childBroadphase = childBroadphase;
+	parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef);
+}
+
+
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax);
+bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
+{
+return
+amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
+amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
+amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
+}
+
+
+
+
+
+
+void	btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+	aabbMin = multiProxy->m_aabbMin;
+	aabbMax = multiProxy->m_aabbMax;
+}
+
+void	btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	for (int i=0;i<m_multiSapProxies.size();i++)
+	{
+		rayCallback.process(m_multiSapProxies[i]);
+	}
+}
+
+
+//#include <stdio.h>
+
+void	btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
+{
+	btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
+	multiProxy->m_aabbMin = aabbMin;
+	multiProxy->m_aabbMax = aabbMax;
+	
+	
+//	bool fullyContained = false;
+//	bool alreadyInSimple = false;
+	
+
+
+	
+	struct MyNodeOverlapCallback : public btNodeOverlapCallback
+	{
+		btMultiSapBroadphase*	m_multiSap;
+		btMultiSapProxy*		m_multiProxy;
+		btDispatcher*			m_dispatcher;
+
+		MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
+			:m_multiSap(multiSap),
+			m_multiProxy(multiProxy),
+			m_dispatcher(dispatcher)
+		{
+
+		}
+
+		virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
+		{
+			btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
+
+			int containingBroadphaseIndex = -1;
+			//already found?
+			for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
+			{
+
+				if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+				{
+					containingBroadphaseIndex = i;
+					break;
+				}
+			}
+			if (containingBroadphaseIndex<0)
+			{
+				//add it
+				btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy);
+				m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
+
+			}
+		}
+	};
+
+	MyNodeOverlapCallback	myNodeCallback(this,multiProxy,dispatcher);
+
+
+
+	
+	m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+	int i;
+
+	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+	{
+		btVector3 worldAabbMin,worldAabbMax;
+		multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+		bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+		if (!overlapsBroadphase)
+		{
+			//remove it now
+			btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
+
+			btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+			bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+			
+			multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
+			multiProxy->m_bridgeProxies.pop_back();
+
+		}
+	}
+
+
+	/*
+
+	if (1)
+	{
+
+		//find broadphase that contain this multiProxy
+		int numChildBroadphases = getBroadphaseArray().size();
+		for (int i=0;i<numChildBroadphases;i++)
+		{
+			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+			btVector3 worldAabbMin,worldAabbMax;
+			childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
+			bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+			
+		//	fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
+			int containingBroadphaseIndex = -1;
+			
+			//if already contains this
+			
+			for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
+			{
+				if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
+				{
+					containingBroadphaseIndex = i;
+				}
+				alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
+			}
+
+			if (overlapsBroadphase)
+			{
+				if (containingBroadphaseIndex<0)
+				{
+					btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+					childProxy->m_multiSapParentProxy = multiProxy;
+					addToChildBroadphase(multiProxy,childProxy,childBroadphase);
+				}
+			} else
+			{
+				if (containingBroadphaseIndex>=0)
+				{
+					//remove
+					btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
+
+					btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
+					bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
+					
+					multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
+					multiProxy->m_bridgeProxies.pop_back();
+				}
+			}
+		}
+
+
+		///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force)
+		///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
+		if (0)//!multiProxy->m_bridgeProxies.size())
+		{
+			///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+			///this is needed to be able to calculate the aabb overlap
+			btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+			childProxy->m_multiSapParentProxy = multiProxy;
+			addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+		}
+	}
+
+	if (!multiProxy->m_bridgeProxies.size())
+	{
+		///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision
+		///this is needed to be able to calculate the aabb overlap
+		btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
+		childProxy->m_multiSapParentProxy = multiProxy;
+		addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
+	}
+*/
+
+
+	//update
+	for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
+	{
+		btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
+		bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
+	}
+
+}
+bool stopUpdating=false;
+
+
+
+class btMultiSapBroadphasePairSortPredicate
+{
+	public:
+
+		bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
+		{
+				btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0;
+				btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0;
+				btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0;
+				btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
+
+				 return aProxy0 > bProxy0 || 
+					(aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
+					(aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); 
+		}
+};
+
+
+        ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+void    btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+
+//	m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
+
+	if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
+	{
+	
+		btBroadphasePairArray&	overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
+
+	//	quicksort(overlappingPairArray,0,overlappingPairArray.size());
+
+		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+	//	overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+
+		
+		int i;
+
+		btBroadphasePair previousPair;
+		previousPair.m_pProxy0 = 0;
+		previousPair.m_pProxy1 = 0;
+		previousPair.m_algorithm = 0;
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			btBroadphasePair& pair = overlappingPairArray[i];
+
+			btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
+			btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0;
+			btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
+			btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
+
+			bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
+			
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;//callback->processOverlap(pair);
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
+
+		//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+		//		m_overlappingPairArray.pop_back();
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				m_invalidPair++;
+				gOverlappingPairs--;
+			} 
+			
+		}
+
+	///if you don't like to skip the invalid pairs in the array, execute following code:
+	#define CLEAN_INVALID_PAIRS 1
+	#ifdef CLEAN_INVALID_PAIRS
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		//overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
+		overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
+
+		overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+		m_invalidPair = 0;
+	#endif//CLEAN_INVALID_PAIRS
+		
+		//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
+	}
+
+
+}
+
+
+bool	btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
+{
+	btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
+		btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
+
+		return	TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
+			multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
+		
+}
+
+
+void	btMultiSapBroadphase::printStats()
+{
+/*	printf("---------------------------------\n");
+	
+		printf("btMultiSapBroadphase.h\n");
+		printf("numHandles = %d\n",m_multiSapProxies.size());
+			//find broadphase that contain this multiProxy
+		int numChildBroadphases = getBroadphaseArray().size();
+		for (int i=0;i<numChildBroadphases;i++)
+		{
+
+			btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
+			childBroadphase->printStats();
+
+		}
+		*/
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h	(revision 1963)
@@ -0,0 +1,147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_MULTI_SAP_BROADPHASE
+#define BT_MULTI_SAP_BROADPHASE
+
+#include "btBroadphaseInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btOverlappingPairCache.h"
+
+
+class btBroadphaseInterface;
+class btSimpleBroadphase;
+
+
+typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
+
+///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases.
+///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time.
+///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
+///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
+///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+class btMultiSapBroadphase :public btBroadphaseInterface
+{
+	btSapBroadphaseArray	m_sapBroadphases;
+	
+	btSimpleBroadphase*		m_simpleBroadphase;
+
+	btOverlappingPairCache*	m_overlappingPairs;
+
+	class btQuantizedBvh*			m_optimizedAabbTree;
+
+
+	bool					m_ownsPairCache;
+	
+	btOverlapFilterCallback*	m_filterCallback;
+
+	int			m_invalidPair;
+
+	struct	btBridgeProxy
+	{
+		btBroadphaseProxy*		m_childProxy;
+		btBroadphaseInterface*	m_childBroadphase;
+	};
+
+
+public:
+
+	struct	btMultiSapProxy	: public btBroadphaseProxy
+	{
+
+		///array with all the entries that this proxy belongs to
+		btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
+		btVector3	m_aabbMin;
+		btVector3	m_aabbMax;
+
+		int	m_shapeType;
+
+/*		void*	m_userPtr;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+*/
+		btMultiSapProxy(const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
+			:btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
+			m_aabbMin(aabbMin),
+			m_aabbMax(aabbMax),
+			m_shapeType(shapeType)
+		{
+			m_multiSapParentProxy =this;
+		}
+
+		
+	};
+
+protected:
+
+
+	btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
+
+public:
+
+	btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
+
+
+	btSapBroadphaseArray&	getBroadphaseArray()
+	{
+		return m_sapBroadphases;
+	}
+
+	const btSapBroadphaseArray&	getBroadphaseArray() const
+	{
+		return m_sapBroadphases;
+	}
+
+	virtual ~btMultiSapBroadphase();
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+
+	void	addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface*	childBroadphase);
+
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
+
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	virtual	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_overlappingPairs;
+	}
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_overlappingPairs;
+	}
+
+	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+	///will add some transform later
+	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		aabbMin.setValue(-1e30f,-1e30f,-1e30f);
+		aabbMax.setValue(1e30f,1e30f,1e30f);
+	}
+
+	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
+
+	virtual void	printStats();
+
+	void quicksort (btBroadphasePairArray& a, int lo, int hi);
+
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp	(revision 1963)
@@ -0,0 +1,579 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btOverlappingPairCache.h"
+
+#include "btDispatcher.h"
+#include "btCollisionAlgorithm.h"
+
+#include <stdio.h>
+
+int	gOverlappingPairs = 0;
+
+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
+
+
+
+btHashedOverlappingPairCache::btHashedOverlappingPairCache():
+	m_overlapFilterCallback(0),
+	m_blockedForChanges(false)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+
+btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
+{
+	//todo/test: show we erase/delete data, or is it automatic
+}
+
+
+
+void	btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~btCollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+		}
+	}
+}
+
+
+
+
+void	btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+	class	CleanPairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_cleanProxy;
+		btOverlappingPairCache*	m_pairCache;
+		btDispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
+			:m_cleanProxy(cleanProxy),
+			m_pairCache(pairCache),
+			m_dispatcher(dispatcher)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			if ((pair.m_pProxy0 == m_cleanProxy) ||
+				(pair.m_pProxy1 == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+			}
+			return false;
+		}
+		
+	};
+
+	CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+
+
+void	btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+	class	RemovePairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+			:m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+				(pair.m_pProxy1 == m_obsoleteProxy));
+		}
+		
+	};
+
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+
+
+
+
+btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	gFindPairs++;
+	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();
+
+	/*if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+	if (hash >= m_hashTable.size())
+	{
+		return NULL;
+	}
+
+	int index = m_hashTable[hash];
+	while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == BT_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void	btHashedOverlappingPairCache::growTables()
+{
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+
+		int i;
+
+		for (i= 0; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = BT_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = BT_NULL_PAIR;
+		}
+
+		for(i=0;i<curHashtableSize;i++)
+		{
+	
+			const btBroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.m_pProxy0->getUid();
+			int proxyId2 = pair.m_pProxy1->getUid();
+			/*if (proxyId1 > proxyId2) 
+				btSwap(proxyId1, proxyId2);*/
+			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
+{
+	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();
+
+	/*if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+
+
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+	/*for(int i=0;i<m_overlappingPairArray.size();++i)
+		{
+		if(	(m_overlappingPairArray[i].m_pProxy0==proxy0)&&
+			(m_overlappingPairArray[i].m_pProxy1==proxy1))
+			{
+			printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
+			internalFindPair(proxy0, proxy1, hash);
+			}
+		}*/
+	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
+	void* mem = &m_overlappingPairArray.expand();
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	}
+	
+	pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+//	pair->m_pProxy0 = proxy0;
+//	pair->m_pProxy1 = proxy1;
+	pair->m_algorithm = 0;
+	pair->m_userInfo = 0;
+	
+
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = count;
+
+	return pair;
+}
+
+
+
+void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+{
+	gRemovePairs++;
+	if(proxy0>proxy1) btSwap(proxy0,proxy1);
+	int proxyId1 = proxy0->getUid();
+	int proxyId2 = proxy1->getUid();
+
+	/*if (proxyId1 > proxyId2) 
+		btSwap(proxyId1, proxyId2);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+	btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	cleanOverlappingPair(*pair,dispatcher);
+
+	void* userData = pair->m_userInfo;
+
+	btAssert(pair->m_pProxy0->getUid() == proxyId1);
+	btAssert(pair->m_pProxy1->getUid() == proxyId2);
+
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
+	btAssert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int index = m_hashTable[hash];
+	btAssert(index != BT_NULL_PAIR);
+
+	int previous = BT_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_NULL_PAIR)
+	{
+		btAssert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return userData;
+	}
+
+	// Remove the last pair from the hash table.
+	const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+		/* missing swap here too, Nat. */ 
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
+
+	index = m_hashTable[lastHash];
+	btAssert(index != BT_NULL_PAIR);
+
+	previous = BT_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_NULL_PAIR)
+	{
+		btAssert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return userData;
+}
+//#include <stdio.h>
+
+void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+	int i;
+
+//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher);
+
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+void*	btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
+{
+	if (!hasDeferredRemoval())
+	{
+		btBroadphasePair findPair(*proxy0,*proxy1);
+
+		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+		if (findIndex < m_overlappingPairArray.size())
+		{
+			gOverlappingPairs--;
+			btBroadphasePair& pair = m_overlappingPairArray[findIndex];
+			void* userData = pair.m_userInfo;
+			cleanOverlappingPair(pair,dispatcher);
+			
+			m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+			m_overlappingPairArray.pop_back();
+			return userData;
+		}
+	}
+
+	return 0;
+}
+
+
+
+
+
+
+
+
+btBroadphasePair*	btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	//don't add overlap with own
+	assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+	
+	void* mem = &m_overlappingPairArray.expand();
+	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+	gOverlappingPairs++;
+	gAddedPairs++;
+	return pair;
+	
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ btBroadphasePair*	btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+
+	btBroadphasePair tmpPair(*proxy0,*proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//assert(it != m_overlappingPairSet.end());
+		 btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+//#include <stdio.h>
+
+void	btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+
+	int i;
+
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		btBroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			cleanOverlappingPair(*pair,dispatcher);
+
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
+			m_overlappingPairArray.pop_back();
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+
+btSortedOverlappingPairCache::btSortedOverlappingPairCache():
+	m_blockedForChanges(false),
+	m_hasDeferredRemoval(true),
+	m_overlapFilterCallback(0)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
+{
+	//todo/test: show we erase/delete data, or is it automatic
+}
+
+void	btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~btCollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+			gRemovePairs--;
+		}
+	}
+}
+
+
+void	btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+	class	CleanPairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_cleanProxy;
+		btOverlappingPairCache*	m_pairCache;
+		btDispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher)
+			:m_cleanProxy(cleanProxy),
+			m_pairCache(pairCache),
+			m_dispatcher(dispatcher)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			if ((pair.m_pProxy0 == m_cleanProxy) ||
+				(pair.m_pProxy1 == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+			}
+			return false;
+		}
+		
+	};
+
+	CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+void	btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
+{
+
+	class	RemovePairCallback : public btOverlapCallback
+	{
+		btBroadphaseProxy* m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(btBroadphaseProxy* obsoleteProxy)
+			:m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual	bool	processOverlap(btBroadphasePair& pair)
+		{
+			return ((pair.m_pProxy0 == m_obsoleteProxy) ||
+				(pair.m_pProxy1 == m_obsoleteProxy));
+		}
+		
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback,dispatcher);
+}
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h	(revision 1963)
@@ -0,0 +1,437 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CACHE_H
+#define OVERLAPPING_PAIR_CACHE_H
+
+
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.h"
+
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+
+typedef btAlignedObjectArray<btBroadphasePair>	btBroadphasePairArray;
+
+struct	btOverlapCallback
+{
+	virtual ~btOverlapCallback()
+	{}
+	//return true for deletion of the pair
+	virtual bool	processOverlap(btBroadphasePair& pair) = 0;
+
+};
+
+struct btOverlapFilterCallback
+{
+	virtual ~btOverlapFilterCallback()
+	{}
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
+};
+
+
+
+
+
+
+
+extern int gRemovePairs;
+extern int gAddedPairs;
+extern int gFindPairs;
+
+const int BT_NULL_PAIR=0xffffffff;
+
+///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
+///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
+class btOverlappingPairCache : public btOverlappingPairCallback
+{
+public:
+	virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
+
+	virtual btBroadphasePair*	getOverlappingPairArrayPtr() = 0;
+	
+	virtual const btBroadphasePair*	getOverlappingPairArrayPtr() const = 0;
+
+	virtual btBroadphasePairArray&	getOverlappingPairArray() = 0;
+
+	virtual	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
+
+	virtual int getNumOverlappingPairs() const = 0;
+
+	virtual void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0;
+
+	virtual	void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
+
+	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
+
+	virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
+
+	virtual bool	hasDeferredRemoval() = 0;
+
+};
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+class btHashedOverlappingPairCache : public btOverlappingPairCache
+{
+	btBroadphasePairArray	m_overlappingPairArray;
+	btOverlapFilterCallback* m_overlapFilterCallback;
+	bool		m_blockedForChanges;
+
+
+public:
+	btHashedOverlappingPairCache();
+	virtual ~btHashedOverlappingPairCache();
+
+	
+	void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+	
+	SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+		
+		return collides;
+	}
+
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	virtual btBroadphasePair* 	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		gAddedPairs++;
+
+		if (!needsBroadphaseCollision(proxy0,proxy1))
+			return 0;
+
+		return internalAddPair(proxy0,proxy1);
+	}
+
+	
+
+	void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+	
+	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	btBroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const btBroadphasePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+
+
+
+	btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+//	btBroadphasePair* GetPairs() { return m_pairs; }
+
+	btOverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	
+	btBroadphasePair* 	internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+	void	growTables();
+
+	SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
+	{	
+		return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+	{
+		int key = (proxyId2 << 16) | proxyId1;
+		key = ~key + (key << 15);
+		key = key ^ (key >> 12);
+		key = key + (key << 2);
+		key = key ^ (key >> 4);
+		key = key * 2057;
+		key = key ^ (key >> 16);
+		return key;
+	}
+	*/
+
+
+	
+	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	{
+		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return static_cast<unsigned int>(key);
+	}
+	
+
+
+
+
+	SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
+	{
+		int proxyId1 = proxy0->getUid();
+		int proxyId2 = proxy1->getUid();
+		#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+		if (proxyId1 > proxyId2) 
+			btSwap(proxyId1, proxyId2);
+		#endif
+
+		int index = m_hashTable[hash];
+		
+		while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == BT_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		btAssert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	virtual bool	hasDeferredRemoval()
+	{
+		return false;
+	}
+
+public:
+	
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	
+};
+
+
+
+
+///btSortedOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
+class	btSortedOverlappingPairCache : public btOverlappingPairCache
+{
+	protected:
+		//avoid brute-force finding all the time
+		btBroadphasePairArray	m_overlappingPairArray;
+
+		//during the dispatch, check that user doesn't destroy/create proxy
+		bool		m_blockedForChanges;
+
+		///by default, do the removal during the pair traversal
+		bool		m_hasDeferredRemoval;
+		
+		//if set, use the callback instead of the built in filter in needBroadphaseCollision
+		btOverlapFilterCallback* m_overlapFilterCallback;
+
+	public:
+			
+		btSortedOverlappingPairCache();	
+		virtual ~btSortedOverlappingPairCache();
+
+		virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
+
+		void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
+
+		void	cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
+		
+		btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+		btBroadphasePair*	findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+			
+		
+		void	cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+		void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+
+
+		inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
+		{
+			if (m_overlapFilterCallback)
+				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+			bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+			collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			
+			return collides;
+		}
+		
+		btBroadphasePairArray&	getOverlappingPairArray()
+		{
+			return m_overlappingPairArray;
+		}
+
+		const btBroadphasePairArray&	getOverlappingPairArray() const
+		{
+			return m_overlappingPairArray;
+		}
+
+		
+
+
+		btBroadphasePair*	getOverlappingPairArrayPtr()
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		const btBroadphasePair*	getOverlappingPairArrayPtr() const
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		int	getNumOverlappingPairs() const
+		{
+			return m_overlappingPairArray.size();
+		}
+		
+		btOverlapFilterCallback* getOverlapFilterCallback()
+		{
+			return m_overlapFilterCallback;
+		}
+
+		void setOverlapFilterCallback(btOverlapFilterCallback* callback)
+		{
+			m_overlapFilterCallback = callback;
+		}
+
+		virtual bool	hasDeferredRemoval()
+		{
+			return m_hasDeferredRemoval;
+		}
+
+
+};
+
+
+
+///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and testing.
+class btNullPairCache : public btOverlappingPairCache
+{
+
+	btBroadphasePairArray	m_overlappingPairArray;
+
+public:
+
+	virtual btBroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+	const btBroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+	btBroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+	
+	virtual	void	cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
+	{
+
+	}
+
+	virtual int getNumOverlappingPairs() const
+	{
+		return 0;
+	}
+
+	virtual void	cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
+	{
+
+	}
+
+	virtual	void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
+	{
+	}
+
+	virtual void	processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual bool	hasDeferredRemoval()
+	{
+		return true;
+	}
+
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/)
+	{
+		return 0;
+	}
+
+	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+	{
+	}
+
+
+};
+
+
+#endif //OVERLAPPING_PAIR_CACHE_H
+
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h	(revision 1963)
@@ -0,0 +1,40 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OVERLAPPING_PAIR_CALLBACK_H
+#define OVERLAPPING_PAIR_CALLBACK_H
+
+class btDispatcher;
+struct  btBroadphasePair;
+
+///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache.
+class btOverlappingPairCallback
+{
+public:
+	virtual ~btOverlappingPairCallback()
+	{
+
+	}
+	
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0;
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0;
+
+	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0;
+
+};
+
+#endif //OVERLAPPING_PAIR_CALLBACK_H
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 1963)
@@ -0,0 +1,1146 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btQuantizedBvh.h"
+
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define RAYAABB2
+
+btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), 
+					//m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
+					m_traversalMode(TRAVERSAL_STACKLESS)
+					//m_traversalMode(TRAVERSAL_RECURSIVE)
+					,m_subtreeHeaderCount(0) //PCK: add this line
+{
+	m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
+	m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+}
+
+
+
+
+
+void btQuantizedBvh::buildInternal()
+{
+	///assumes that caller filled in the m_quantizedLeafNodes
+	m_useQuantization = true;
+	int numLeafNodes = 0;
+	
+	if (m_useQuantization)
+	{
+		//now we have an array of leafnodes in m_leafNodes
+		numLeafNodes = m_quantizedLeafNodes.size();
+
+		m_quantizedContiguousNodes.resize(2*numLeafNodes);
+
+	}
+
+	m_curNodeIndex = 0;
+
+	buildTree(0,numLeafNodes);
+
+	///if the entire tree is small then subtree size, we need to create a header info for the tree
+	if(m_useQuantization && !m_SubtreeHeaders.size())
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
+		subtree.m_rootNodeIndex = 0;
+		subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
+	}
+
+	//PCK: update the copy of the size
+	m_subtreeHeaderCount = m_SubtreeHeaders.size();
+
+	//PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
+	m_quantizedLeafNodes.clear();
+	m_leafNodes.clear();
+}
+
+
+
+///just for debugging, to visualize the individual patches/subtrees
+#ifdef DEBUG_PATCH_COLORS
+btVector3 color[4]=
+{
+	btVector3(255,0,0),
+	btVector3(0,255,0),
+	btVector3(0,0,255),
+	btVector3(0,255,255)
+};
+#endif //DEBUG_PATCH_COLORS
+
+
+
+void	btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin)
+{
+	//enlarge the AABB to avoid division by zero when initializing the quantization values
+	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+	m_bvhAabbMin = bvhAabbMin - clampValue;
+	m_bvhAabbMax = bvhAabbMax + clampValue;
+	btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+	m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+	m_useQuantization = true;
+}
+
+
+
+
+btQuantizedBvh::~btQuantizedBvh()
+{
+}
+
+#ifdef DEBUG_TREE_BUILDING
+int gStackDepth = 0;
+int gMaxStackDepth = 0;
+#endif //DEBUG_TREE_BUILDING
+
+void	btQuantizedBvh::buildTree	(int startIndex,int endIndex)
+{
+#ifdef DEBUG_TREE_BUILDING
+	gStackDepth++;
+	if (gStackDepth > gMaxStackDepth)
+		gMaxStackDepth = gStackDepth;
+#endif //DEBUG_TREE_BUILDING
+
+
+	int splitAxis, splitIndex, i;
+	int numIndices =endIndex-startIndex;
+	int curIndex = m_curNodeIndex;
+
+	btAssert(numIndices>0);
+
+	if (numIndices==1)
+	{
+#ifdef DEBUG_TREE_BUILDING
+		gStackDepth--;
+#endif //DEBUG_TREE_BUILDING
+		
+		assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
+
+		m_curNodeIndex++;
+		return;	
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+	
+	splitAxis = calcSplittingAxis(startIndex,endIndex);
+
+	splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
+
+	int internalNodeIndex = m_curNodeIndex;
+	
+	//set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
+	//the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
+	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
+	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
+	
+	
+	for (i=startIndex;i<endIndex;i++)
+	{
+		mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
+	}
+
+	m_curNodeIndex++;
+	
+
+	//internalNode->m_escapeIndex;
+	
+	int leftChildNodexIndex = m_curNodeIndex;
+
+	//build left child tree
+	buildTree(startIndex,splitIndex);
+
+	int rightChildNodexIndex = m_curNodeIndex;
+	//build right child tree
+	buildTree(splitIndex,endIndex);
+
+#ifdef DEBUG_TREE_BUILDING
+	gStackDepth--;
+#endif //DEBUG_TREE_BUILDING
+
+	int escapeIndex = m_curNodeIndex - curIndex;
+
+	if (m_useQuantization)
+	{
+		//escapeIndex is the number of nodes of this subtree
+		const int sizeQuantizedNode =sizeof(btQuantizedBvhNode);
+		const int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
+		if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES)
+		{
+			updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
+		}
+	} else
+	{
+
+	}
+
+	setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
+
+}
+
+void	btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex)
+{
+	btAssert(m_useQuantization);
+
+	btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
+	int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex();
+	int leftSubTreeSizeInBytes =  leftSubTreeSize * static_cast<int>(sizeof(btQuantizedBvhNode));
+	
+	btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
+	int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex();
+	int rightSubTreeSizeInBytes =  rightSubTreeSize *  static_cast<int>(sizeof(btQuantizedBvhNode));
+
+	if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(leftChildNode);
+		subtree.m_rootNodeIndex = leftChildNodexIndex;
+		subtree.m_subtreeSize = leftSubTreeSize;
+	}
+
+	if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(rightChildNode);
+		subtree.m_rootNodeIndex = rightChildNodexIndex;
+		subtree.m_subtreeSize = rightSubTreeSize;
+	}
+
+	//PCK: update the copy of the size
+	m_subtreeHeaderCount = m_SubtreeHeaders.size();
+}
+
+
+int	btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+	btScalar splitValue;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+	
+	splitValue = means[splitAxis];
+	
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			swapLeafNodes(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)	
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	int rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+	
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	(void)unbal;
+	btAssert(!unbal);
+
+	return splitIndex;
+}
+
+
+int	btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex)
+{
+	int i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+		
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+	
+	return variance.maxAxis();
+}
+
+
+
+void	btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	//either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
+
+	if (m_useQuantization)
+	{
+		///quantize query AABB
+		unsigned short int quantizedQueryAabbMin[3];
+		unsigned short int quantizedQueryAabbMax[3];
+		quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
+		quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
+
+		switch (m_traversalMode)
+		{
+		case TRAVERSAL_STACKLESS:
+				walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex);
+			break;
+		case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
+				walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			break;
+		case TRAVERSAL_RECURSIVE:
+			{
+				const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
+				walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+			}
+			break;
+		default:
+			//unsupported
+			btAssert(0);
+		}
+	} else
+	{
+		walkStacklessTree(nodeCallback,aabbMin,aabbMax);
+	}
+}
+
+
+int maxIterations = 0;
+
+
+void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	btAssert(!m_useQuantization);
+
+	const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned aabbOverlap;
+
+	while (curIndex < m_curNodeIndex)
+	{
+		//catch bugs in tree data
+		btAssert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+		aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		isLeafNode = rootNode->m_escapeIndex == -1;
+		
+		//PCK: unsigned instead of bool
+		if (isLeafNode && (aabbOverlap != 0))
+		{
+			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+		} 
+		
+		//PCK: unsigned instead of bool
+		if ((aabbOverlap != 0) || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->m_escapeIndex;
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+/*
+///this was the original recursive traversal, before we optimized towards stackless traversal
+void	btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+	if (aabbOverlap)
+	{
+		isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+		if (isLeafNode)
+		{
+			nodeCallback->processNode(rootNode);
+		} else
+		{
+			walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
+			walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
+		}
+	}
+
+}
+*/
+
+void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+{
+	btAssert(m_useQuantization);
+	
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned aabbOverlap;
+
+	//PCK: unsigned instead of bool
+	aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
+	isLeafNode = currentNode->isLeafNode();
+		
+	//PCK: unsigned instead of bool
+	if (aabbOverlap != 0)
+	{
+		if (isLeafNode)
+		{
+			nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex());
+		} else
+		{
+			//process left and right children
+			const btQuantizedBvhNode* leftChildNode = currentNode+1;
+			walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+
+			const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex();
+			walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
+		}
+	}		
+}
+
+
+
+void	btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+{
+	btAssert(!m_useQuantization);
+
+	const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+	int escapeIndex, curIndex = 0;
+	int walkIterations = 0;
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned aabbOverlap=0;
+	unsigned rayBoxOverlap=0;
+	btScalar lambda_max = 1.0;
+	
+		/* Quick pruning by quantized box */
+	btVector3 rayAabbMin = raySource;
+	btVector3 rayAabbMax = raySource;
+	rayAabbMin.setMin(rayTarget);
+	rayAabbMax.setMax(rayTarget);
+
+	/* Add box cast extents to bounding box */
+	rayAabbMin += aabbMin;
+	rayAabbMax += aabbMax;
+
+#ifdef RAYAABB2
+	btVector3 rayFrom = raySource;
+	btVector3 rayDir = (rayTarget-raySource);
+	rayDir.normalize ();
+	lambda_max = rayDir.dot(rayTarget-raySource);
+	///what about division by zero? --> just set rayDirection[i] to 1.0
+	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 sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+#endif
+
+	btVector3 bounds[2];
+
+	while (curIndex < m_curNodeIndex)
+	{
+		btScalar param = 1.0;
+		//catch bugs in tree data
+		btAssert (walkIterations < m_curNodeIndex);
+
+		walkIterations++;
+
+		bounds[0] = rootNode->m_aabbMinOrg;
+		bounds[1] = rootNode->m_aabbMaxOrg;
+		/* Add box cast extents */
+		bounds[0] += aabbMin;
+		bounds[1] += aabbMax;
+
+		aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+		//perhaps profile if it is worth doing the aabbOverlap test first
+
+#ifdef RAYAABB2
+			///careful with this check: need to check division by zero (above) and fix the unQuantize method
+			///thanks Joerg/hiker for the reproduction case!
+			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
+		rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
+
+#else
+		btVector3 normal;
+		rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
+#endif
+
+		isLeafNode = rootNode->m_escapeIndex == -1;
+		
+		//PCK: unsigned instead of bool
+		if (isLeafNode && (rayBoxOverlap != 0))
+		{
+			nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+		} 
+		
+		//PCK: unsigned instead of bool
+		if ((rayBoxOverlap != 0) || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->m_escapeIndex;
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+
+
+void	btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+{
+	btAssert(m_useQuantization);
+	
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+	(void)subTreeSize;
+
+	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
+	int escapeIndex;
+	
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned boxBoxOverlap = 0;
+	unsigned rayBoxOverlap = 0;
+
+	btScalar lambda_max = 1.0;
+
+#ifdef RAYAABB2
+	btVector3 rayFrom = raySource;
+	btVector3 rayDirection = (rayTarget-raySource);
+	rayDirection.normalize ();
+	lambda_max = rayDirection.dot(rayTarget-raySource);
+	///what about division by zero? --> just set rayDirection[i] to 1.0
+	rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0];
+	rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1];
+	rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2];
+	unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
+#endif
+
+	/* Quick pruning by quantized box */
+	btVector3 rayAabbMin = raySource;
+	btVector3 rayAabbMax = raySource;
+	rayAabbMin.setMin(rayTarget);
+	rayAabbMax.setMax(rayTarget);
+
+	/* Add box cast extents to bounding box */
+	rayAabbMin += aabbMin;
+	rayAabbMax += aabbMax;
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0);
+	quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1);
+
+	while (curIndex < endNodeIndex)
+	{
+
+//#define VISUALLY_ANALYZE_BVH 1
+#ifdef VISUALLY_ANALYZE_BVH
+		//some code snippet to debugDraw aabb, to visually analyze bvh structure
+		static int drawPatch = 0;
+		//need some global access to a debugDrawer
+		extern btIDebugDraw* debugDrawerPtr;
+		if (curIndex==drawPatch)
+		{
+			btVector3 aabbMin,aabbMax;
+			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
+			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
+			btVector3	color(1,0,0);
+			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+		}
+#endif//VISUALLY_ANALYZE_BVH
+
+		//catch bugs in tree data
+		btAssert (walkIterations < subTreeSize);
+
+		walkIterations++;
+		//PCK: unsigned instead of bool
+		// only interested if this is closer than any previous hit
+		btScalar param = 1.0;
+		rayBoxOverlap = 0;
+		boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		isLeafNode = rootNode->isLeafNode();
+		if (boxBoxOverlap)
+		{
+			btVector3 bounds[2];
+			bounds[0] = unQuantize(rootNode->m_quantizedAabbMin);
+			bounds[1] = unQuantize(rootNode->m_quantizedAabbMax);
+			/* Add box cast extents */
+			bounds[0] += aabbMin;
+			bounds[1] += aabbMax;
+			btVector3 normal;
+#if 0
+			bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
+			bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
+			if (ra2 != ra)
+			{
+				printf("functions don't match\n");
+			}
+#endif
+#ifdef RAYAABB2
+			///careful with this check: need to check division by zero (above) and fix the unQuantize method
+			///thanks Joerg/hiker for the reproduction case!
+			///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
+
+			//BT_PROFILE("btRayAabb2");
+			rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
+			
+#else
+			rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
+#endif
+		}
+		
+		if (isLeafNode && rayBoxOverlap)
+		{
+			nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+		}
+		
+		//PCK: unsigned instead of bool
+		if ((rayBoxOverlap != 0) || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+void	btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const
+{
+	btAssert(m_useQuantization);
+	
+	int curIndex = startNodeIndex;
+	int walkIterations = 0;
+	int subTreeSize = endNodeIndex - startNodeIndex;
+	(void)subTreeSize;
+
+	const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
+	int escapeIndex;
+	
+	bool isLeafNode;
+	//PCK: unsigned instead of bool
+	unsigned aabbOverlap;
+
+	while (curIndex < endNodeIndex)
+	{
+
+//#define VISUALLY_ANALYZE_BVH 1
+#ifdef VISUALLY_ANALYZE_BVH
+		//some code snippet to debugDraw aabb, to visually analyze bvh structure
+		static int drawPatch = 0;
+		//need some global access to a debugDrawer
+		extern btIDebugDraw* debugDrawerPtr;
+		if (curIndex==drawPatch)
+		{
+			btVector3 aabbMin,aabbMax;
+			aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
+			aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
+			btVector3	color(1,0,0);
+			debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
+		}
+#endif//VISUALLY_ANALYZE_BVH
+
+		//catch bugs in tree data
+		btAssert (walkIterations < subTreeSize);
+
+		walkIterations++;
+		//PCK: unsigned instead of bool
+		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+		isLeafNode = rootNode->isLeafNode();
+		
+		if (isLeafNode && aabbOverlap)
+		{
+			nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+		} 
+		
+		//PCK: unsigned instead of bool
+		if ((aabbOverlap != 0) || isLeafNode)
+		{
+			rootNode++;
+			curIndex++;
+		} else
+		{
+			escapeIndex = rootNode->getEscapeIndex();
+			rootNode += escapeIndex;
+			curIndex += escapeIndex;
+		}
+	}
+	if (maxIterations < walkIterations)
+		maxIterations = walkIterations;
+
+}
+
+//This traversal can be called from Playstation 3 SPU
+void	btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
+{
+	btAssert(m_useQuantization);
+
+	int i;
+
+
+	for (i=0;i<this->m_SubtreeHeaders.size();i++)
+	{
+		const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+
+		//PCK: unsigned instead of bool
+		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
+				subtree.m_rootNodeIndex,
+				subtree.m_rootNodeIndex+subtree.m_subtreeSize);
+		}
+	}
+}
+
+
+void	btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
+{
+	reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
+}
+
+
+void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	//always use stackless
+
+	if (m_useQuantization)
+	{
+		walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+	}
+	else
+	{
+		walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+	}
+	/*
+	{
+		//recursive traversal
+		btVector3 qaabbMin = raySource;
+		btVector3 qaabbMax = raySource;
+		qaabbMin.setMin(rayTarget);
+		qaabbMax.setMax(rayTarget);
+		qaabbMin += aabbMin;
+		qaabbMax += aabbMax;
+		reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
+	}
+	*/
+
+}
+
+
+void	btQuantizedBvh::swapLeafNodes(int i,int splitIndex)
+{
+	if (m_useQuantization)
+	{
+			btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
+			m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
+			m_quantizedLeafNodes[splitIndex] = tmp;
+	} else
+	{
+			btOptimizedBvhNode tmp = m_leafNodes[i];
+			m_leafNodes[i] = m_leafNodes[splitIndex];
+			m_leafNodes[splitIndex] = tmp;
+	}
+}
+
+void	btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
+{
+	if (m_useQuantization)
+	{
+		m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
+	} else
+	{
+		m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
+	}
+}
+
+//PCK: include
+#include <new>
+
+//PCK: consts
+static const unsigned BVH_ALIGNMENT = 16;
+static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1;
+
+static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
+
+
+
+unsigned int btQuantizedBvh::getAlignmentSerializationPadding()
+{
+	// I changed this to 0 since the extra padding is not needed or used.
+	return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT;
+}
+
+unsigned btQuantizedBvh::calculateSerializeBufferSize()
+{
+	unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding();
+	baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount;
+	if (m_useQuantization)
+	{
+		return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode);
+	}
+	return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode);
+}
+
+bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
+{
+	btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
+	m_subtreeHeaderCount = m_SubtreeHeaders.size();
+
+/*	if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	{
+		///check alignedment for buffer?
+		btAssert(0);
+		return false;
+	}
+*/
+
+	btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer;
+
+	// construct the class so the virtual function table, etc will be set up
+	// Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
+	new (targetBvh) btQuantizedBvh;
+
+	if (i_swapEndian)
+	{
+		targetBvh->m_curNodeIndex = static_cast<int>(btSwapEndian(m_curNodeIndex));
+
+
+		btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin);
+		btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax);
+		btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization);
+
+		targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode);
+		targetBvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(m_subtreeHeaderCount));
+	}
+	else
+	{
+		targetBvh->m_curNodeIndex = m_curNodeIndex;
+		targetBvh->m_bvhAabbMin = m_bvhAabbMin;
+		targetBvh->m_bvhAabbMax = m_bvhAabbMax;
+		targetBvh->m_bvhQuantization = m_bvhQuantization;
+		targetBvh->m_traversalMode = m_traversalMode;
+		targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount;
+	}
+
+	targetBvh->m_useQuantization = m_useQuantization;
+
+	unsigned char *nodeData = (unsigned char *)targetBvh;
+	nodeData += sizeof(btQuantizedBvh);
+	
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	nodeData += sizeToAdd;
+	
+	int nodeCount = m_curNodeIndex;
+
+	if (m_useQuantization)
+	{
+		targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+		if (i_swapEndian)
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
+
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
+
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex));
+			}
+		}
+		else
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+	
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0];
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1];
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2];
+
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0];
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1];
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2];
+
+				targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex;
+
+
+			}
+		}
+		nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
+
+		// this clears the pointer in the member variable it doesn't really do anything to the data
+		// it does call the destructor on the contained objects, but they are all classes with no destructor defined
+		// so the memory (which is not freed) is left alone
+		targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0);
+	}
+	else
+	{
+		targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+		if (i_swapEndian)
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+				btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
+				btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
+
+				targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex));
+				targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart));
+				targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex));
+			}
+		}
+		else
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+				targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg;
+				targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg;
+
+				targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex;
+				targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart;
+				targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex;
+			}
+		}
+		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
+
+		// this clears the pointer in the member variable it doesn't really do anything to the data
+		// it does call the destructor on the contained objects, but they are all classes with no destructor defined
+		// so the memory (which is not freed) is left alone
+		targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0);
+	}
+
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	nodeData += sizeToAdd;
+
+	// Now serialize the subtree headers
+	targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount);
+	if (i_swapEndian)
+	{
+		for (int i = 0; i < m_subtreeHeaderCount; i++)
+		{
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+			targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex));
+			targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize));
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_subtreeHeaderCount; i++)
+		{
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+			targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+			targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex);
+			targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize);
+
+			// need to clear padding in destination buffer
+			targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0;
+			targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0;
+			targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0;
+		}
+	}
+	nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount;
+
+	// this clears the pointer in the member variable it doesn't really do anything to the data
+	// it does call the destructor on the contained objects, but they are all classes with no destructor defined
+	// so the memory (which is not freed) is left alone
+	targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0);
+
+	// this wipes the virtual function table pointer at the start of the buffer for the class
+	*((void**)o_alignedDataBuffer) = NULL;
+
+	return true;
+}
+
+btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+{
+
+	if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
+	{
+		return NULL;
+	}
+	btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer;
+
+	if (i_swapEndian)
+	{
+		bvh->m_curNodeIndex = static_cast<int>(btSwapEndian(bvh->m_curNodeIndex));
+
+		btUnSwapVector3Endian(bvh->m_bvhAabbMin);
+		btUnSwapVector3Endian(bvh->m_bvhAabbMax);
+		btUnSwapVector3Endian(bvh->m_bvhQuantization);
+
+		bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode);
+		bvh->m_subtreeHeaderCount = static_cast<int>(btSwapEndian(bvh->m_subtreeHeaderCount));
+	}
+
+	unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize();
+	btAssert(calculatedBufSize <= i_dataBufferSize);
+
+	if (calculatedBufSize > i_dataBufferSize)
+	{
+		return NULL;
+	}
+
+	unsigned char *nodeData = (unsigned char *)bvh;
+	nodeData += sizeof(btQuantizedBvh);
+	
+	unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	nodeData += sizeToAdd;
+	
+	int nodeCount = bvh->m_curNodeIndex;
+
+	// Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor
+	// Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
+	new (bvh) btQuantizedBvh(*bvh, false);
+
+	if (bvh->m_useQuantization)
+	{
+		bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+		if (i_swapEndian)
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
+
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
+
+				bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast<int>(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex));
+			}
+		}
+		nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
+	}
+	else
+	{
+		bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
+
+		if (i_swapEndian)
+		{
+			for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
+			{
+				btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
+				btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
+				
+				bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex));
+				bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart));
+				bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast<int>(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex));
+			}
+		}
+		nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
+	}
+
+	sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
+	nodeData += sizeToAdd;
+
+	// Now serialize the subtree headers
+	bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount);
+	if (i_swapEndian)
+	{
+		for (int i = 0; i < bvh->m_subtreeHeaderCount; i++)
+		{
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
+
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
+			bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
+
+			bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex));
+			bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast<int>(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize));
+		}
+	}
+
+	return bvh;
+}
+
+// Constructor that prevents btVector3's default constructor from being called
+btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) :
+m_bvhAabbMin(self.m_bvhAabbMin),
+m_bvhAabbMax(self.m_bvhAabbMax),
+m_bvhQuantization(self.m_bvhQuantization)
+{
+
+
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h	(revision 1963)
@@ -0,0 +1,487 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef QUANTIZED_BVH_H
+#define QUANTIZED_BVH_H
+
+//#define DEBUG_CHECK_DEQUANTIZATION 1
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+#ifdef __SPU__
+#define printf spu_printf
+#endif //__SPU__
+
+#include <stdio.h>
+#include <stdlib.h>
+#endif //DEBUG_CHECK_DEQUANTIZATION
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
+
+
+//Note: currently we have 16 bytes per quantized node
+#define MAX_SUBTREE_SIZE_IN_BYTES  2048
+
+// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
+// actually) triangles each (since the sign bit is reserved
+#define MAX_NUM_PARTS_IN_BITS 10
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrTriangleIndex;
+
+	bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrTriangleIndex >= 0);
+	}
+	int getEscapeIndex() const
+	{
+		btAssert(!isLeafNode());
+		return -m_escapeIndexOrTriangleIndex;
+	}
+	int	getTriangleIndex() const
+	{
+		btAssert(isLeafNode());
+		// Get only the lower bits where the triangle index is stored
+		return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
+	}
+	int	getPartId() const
+	{
+		btAssert(isLeafNode());
+		// Get only the highest bits where the part index is stored
+		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+	}
+}
+;
+
+/// btOptimizedBvhNode contains both internal and leaf node information.
+/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
+ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	//32 bytes
+	btVector3	m_aabbMinOrg;
+	btVector3	m_aabbMaxOrg;
+
+	//4
+	int	m_escapeIndex;
+
+	//8
+	//for child nodes
+	int	m_subPart;
+	int	m_triangleIndex;
+	int	m_padding[5];//bad, due to alignment
+
+
+};
+
+
+///btBvhSubtreeInfo provides info to gather a subtree of limited size
+ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
+{
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int			m_rootNodeIndex;
+	//4 bytes
+	int			m_subtreeSize;
+	int			m_padding[3];
+
+	btBvhSubtreeInfo()
+	{
+		//memset(&m_padding[0], 0, sizeof(m_padding));
+	}
+
+
+	void	setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
+	{
+		m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
+		m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
+		m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
+		m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
+		m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
+		m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
+	}
+}
+;
+
+
+class btNodeOverlapCallback
+{
+public:
+	virtual ~btNodeOverlapCallback() {};
+
+	virtual void processNode(int subPart, int triangleIndex) = 0;
+};
+
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+
+///for code readability:
+typedef btAlignedObjectArray<btOptimizedBvhNode>	NodeArray;
+typedef btAlignedObjectArray<btQuantizedBvhNode>	QuantizedNodeArray;
+typedef btAlignedObjectArray<btBvhSubtreeInfo>		BvhSubtreeInfoArray;
+
+
+///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
+///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
+///It is recommended to use quantization for better performance and lower memory requirements.
+ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
+{
+protected:
+
+	NodeArray			m_leafNodes;
+	NodeArray			m_contiguousNodes;
+
+	QuantizedNodeArray	m_quantizedLeafNodes;
+	
+	QuantizedNodeArray	m_quantizedContiguousNodes;
+	
+	int					m_curNodeIndex;
+
+
+	//quantization data
+	bool				m_useQuantization;
+	btVector3			m_bvhAabbMin;
+	btVector3			m_bvhAabbMax;
+	btVector3			m_bvhQuantization;
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	enum btTraversalMode
+	{
+		TRAVERSAL_STACKLESS = 0,
+		TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
+		TRAVERSAL_RECURSIVE
+	};
+protected:
+
+	btTraversalMode	m_traversalMode;
+	
+	BvhSubtreeInfoArray		m_SubtreeHeaders;
+
+	//This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
+	int m_subtreeHeaderCount;
+
+
+	///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+	///this might be refactored into a virtual, it is usually not calculated at run-time
+	void	setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
+	{
+		if (m_useQuantization)
+		{
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
+		} else
+		{
+			m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
+
+		}
+	}
+	void	setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
+	{
+		if (m_useQuantization)
+		{
+			quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
+		} else
+		{
+			m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
+		}
+	}
+
+	btVector3 getAabbMin(int nodeIndex) const
+	{
+		if (m_useQuantization)
+		{
+			return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
+		}
+		//non-quantized
+		return m_leafNodes[nodeIndex].m_aabbMinOrg;
+
+	}
+	btVector3 getAabbMax(int nodeIndex) const
+	{
+		if (m_useQuantization)
+		{
+			return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
+		} 
+		//non-quantized
+		return m_leafNodes[nodeIndex].m_aabbMaxOrg;
+		
+	}
+
+	
+	void	setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
+	{
+		if (m_useQuantization)
+		{
+			m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
+		} 
+		else
+		{
+			m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
+		}
+
+	}
+
+	void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) 
+	{
+		if (m_useQuantization)
+		{
+			unsigned short int quantizedAabbMin[3];
+			unsigned short int quantizedAabbMax[3];
+			quantize(quantizedAabbMin,newAabbMin,0);
+			quantize(quantizedAabbMax,newAabbMax,1);
+			for (int i=0;i<3;i++)
+			{
+				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
+					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
+
+				if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
+					m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
+
+			}
+		} else
+		{
+			//non-quantized
+			m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
+			m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);		
+		}
+	}
+
+	void	swapLeafNodes(int firstIndex,int secondIndex);
+
+	void	assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
+
+protected:
+
+	
+
+	void	buildTree	(int startIndex,int endIndex);
+
+	int	calcSplittingAxis(int startIndex,int endIndex);
+
+	int	sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
+	
+	void	walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	void	walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
+	void	walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
+	void	walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
+
+	///tree traversal designed for small-memory processors like PS3 SPU
+	void	walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+
+	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
+	void	walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
+
+	///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
+	void	walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
+	
+
+#define USE_BANCHLESS 1
+#ifdef USE_BANCHLESS
+	//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+	SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	{		
+		return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+			& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+			& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+			1, 0));
+	}
+#else
+	SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
+	{
+		bool overlap = true;
+		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+		return overlap;
+	}
+#endif //USE_BANCHLESS
+
+	void	updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
+
+public:
+	btQuantizedBvh();
+
+	virtual ~btQuantizedBvh();
+
+	
+	///***************************************** expert/internal use only *************************
+	void	setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
+	QuantizedNodeArray&	getLeafNodeArray() {			return	m_quantizedLeafNodes;	}
+	///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
+	void	buildInternal();
+	///***************************************** expert/internal use only *************************
+
+	void	reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+	void	reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
+	void	reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+		SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
+	{
+
+		btAssert(m_useQuantization);
+
+		btAssert(point.getX() <= m_bvhAabbMax.getX());
+		btAssert(point.getY() <= m_bvhAabbMax.getY());
+		btAssert(point.getZ() <= m_bvhAabbMax.getZ());
+
+		btAssert(point.getX() >= m_bvhAabbMin.getX());
+		btAssert(point.getY() >= m_bvhAabbMin.getY());
+		btAssert(point.getZ() >= m_bvhAabbMin.getZ());
+
+		btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
+		///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
+		///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
+		///todo: double-check this
+		if (isMax)
+		{
+			out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
+			out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
+			out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
+		} else
+		{
+			out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
+			out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
+			out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
+		}
+
+
+#ifdef DEBUG_CHECK_DEQUANTIZATION
+		btVector3 newPoint = unQuantize(out);
+		if (isMax)
+		{
+			if (newPoint.getX() < point.getX())
+			{
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+			}
+			if (newPoint.getY() < point.getY())
+			{
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+			}
+			if (newPoint.getZ() < point.getZ())
+			{
+
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+			}
+		} else
+		{
+			if (newPoint.getX() > point.getX())
+			{
+				printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
+			}
+			if (newPoint.getY() > point.getY())
+			{
+				printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
+			}
+			if (newPoint.getZ() > point.getZ())
+			{
+				printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
+			}
+		}
+#endif //DEBUG_CHECK_DEQUANTIZATION
+
+	}
+
+
+	SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
+	{
+
+		btAssert(m_useQuantization);
+
+		btVector3 clampedPoint(point2);
+		clampedPoint.setMax(m_bvhAabbMin);
+		clampedPoint.setMin(m_bvhAabbMax);
+
+		quantize(out,clampedPoint,isMax);
+
+	}
+	
+	SIMD_FORCE_INLINE btVector3	unQuantize(const unsigned short* vecIn) const
+	{
+			btVector3	vecOut;
+			vecOut.setValue(
+			(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
+			(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
+			(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
+			vecOut += m_bvhAabbMin;
+			return vecOut;
+	}
+
+	///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
+	void	setTraversalMode(btTraversalMode	traversalMode)
+	{
+		m_traversalMode = traversalMode;
+	}
+
+
+	SIMD_FORCE_INLINE QuantizedNodeArray&	getQuantizedNodeArray()
+	{	
+		return	m_quantizedContiguousNodes;
+	}
+
+
+	SIMD_FORCE_INLINE BvhSubtreeInfoArray&	getSubtreeInfoArray()
+	{
+		return m_SubtreeHeaders;
+	}
+
+
+	/////Calculate space needed to store BVH for serialization
+	unsigned calculateSerializeBufferSize();
+
+	/// Data buffer MUST be 16 byte aligned
+	virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian);
+
+	///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+	static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
+
+	static unsigned int getAlignmentSerializationPadding();
+
+	SIMD_FORCE_INLINE bool isQuantized()
+	{
+		return m_useQuantization;
+	}
+
+private:
+	// Special "copy" constructor that allows for in-place deserialization
+	// Prevents btVector3's default constructor from being called, but doesn't inialize much else
+	// ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need)
+	btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
+
+}
+;
+
+
+#endif //QUANTIZED_BVH_H
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp	(revision 1963)
@@ -0,0 +1,314 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include <new>
+
+extern int gOverlappingPairs;
+
+void	btSimpleBroadphase::validate()
+{
+	for (int i=0;i<m_numHandles;i++)
+	{
+		for (int j=i+1;j<m_numHandles;j++)
+		{
+			btAssert(&m_pHandles[i] != &m_pHandles[j]);
+		}
+	}
+	
+}
+
+btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
+	:m_pairCache(overlappingPairCache),
+	m_ownsPairCache(false),
+	m_invalidPair(0)
+{
+
+	if (!overlappingPairCache)
+	{
+		void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
+		m_pairCache = new (mem)btHashedOverlappingPairCache();
+		m_ownsPairCache = true;
+	}
+
+	// allocate handles buffer and put all handles on free list
+	m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy)*maxProxies,16);
+	m_pHandles = new(m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
+	m_maxHandles = maxProxies;
+	m_numHandles = 0;
+	m_firstFreeHandle = 0;
+	
+
+	{
+		for (int i = m_firstFreeHandle; i < maxProxies; i++)
+		{
+			m_pHandles[i].SetNextFree(i + 1);
+			m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
+		}
+		m_pHandles[maxProxies - 1].SetNextFree(0);
+	
+	}
+
+}
+
+btSimpleBroadphase::~btSimpleBroadphase()
+{
+	btAlignedFree(m_pHandlesRawPtr);
+
+	if (m_ownsPairCache)
+	{
+		m_pairCache->~btOverlappingPairCache();
+		btAlignedFree(m_pairCache);
+	}
+}
+
+
+btBroadphaseProxy*	btSimpleBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)
+{
+	if (m_numHandles >= m_maxHandles)
+	{
+		btAssert(0);
+		return 0; //should never happen, but don't let the game crash ;-)
+	}
+	btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);
+
+	int newHandleIndex = allocHandle();
+	btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
+
+	return proxy;
+}
+
+class	RemovingOverlapCallback : public btOverlapCallback
+{
+protected:
+	virtual bool	processOverlap(btBroadphasePair& pair)
+	{
+		(void)pair;
+		btAssert(0);
+		return false;
+	}
+};
+
+class RemovePairContainingProxy
+{
+
+	btBroadphaseProxy*	m_targetProxy;
+	public:
+	virtual ~RemovePairContainingProxy()
+	{
+	}
+protected:
+	virtual bool processOverlap(btBroadphasePair& pair)
+	{
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
+		btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
+
+		return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
+	};
+};
+
+void	btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)
+{
+		
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
+		freeHandle(proxy0);
+
+		m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);
+
+		//validate();
+		
+}
+
+void	btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
+{
+	const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+	aabbMin = sbp->m_aabbMin;
+	aabbMax = sbp->m_aabbMax;
+}
+
+void	btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
+{
+	btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
+	sbp->m_aabbMin = aabbMin;
+	sbp->m_aabbMax = aabbMax;
+}
+
+void	btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback)
+{
+	for (int i=0;i<m_numHandles;i++)
+	{
+		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+		rayCallback.process(proxy);
+	}
+}
+
+
+
+	
+
+
+
+bool	btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1)
+{
+	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && 
+		   proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
+		   proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
+
+}
+
+
+
+//then remove non-overlapping ones
+class CheckOverlapCallback : public btOverlapCallback
+{
+public:
+	virtual bool processOverlap(btBroadphasePair& pair)
+	{
+		return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
+	}
+};
+
+void	btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
+{
+	//first check for new overlapping pairs
+	int i,j;
+
+	if (m_numHandles >= 0)
+	{
+
+		for (i=0;i<m_numHandles;i++)
+		{
+			btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
+
+			for (j=i+1;j<m_numHandles;j++)
+			{
+				btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
+				btAssert(proxy0 != proxy1);
+
+				btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+				btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+				if (aabbOverlap(p0,p1))
+				{
+					if ( !m_pairCache->findPair(proxy0,proxy1))
+					{
+						m_pairCache->addOverlappingPair(proxy0,proxy1);
+					}
+				} else
+				{
+					if (!m_pairCache->hasDeferredRemoval())
+					{
+						if ( m_pairCache->findPair(proxy0,proxy1))
+						{
+							m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+						}
+					}
+				}
+			}
+		}
+
+		if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
+		{
+
+			btBroadphasePairArray&	overlappingPairArray = m_pairCache->getOverlappingPairArray();
+
+			//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+			overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
+
+
+			btBroadphasePair previousPair;
+			previousPair.m_pProxy0 = 0;
+			previousPair.m_pProxy1 = 0;
+			previousPair.m_algorithm = 0;
+
+
+			for (i=0;i<overlappingPairArray.size();i++)
+			{
+
+				btBroadphasePair& pair = overlappingPairArray[i];
+
+				bool isDuplicate = (pair == previousPair);
+
+				previousPair = pair;
+
+				bool needsRemoval = false;
+
+				if (!isDuplicate)
+				{
+					bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
+
+					if (hasOverlap)
+					{
+						needsRemoval = false;//callback->processOverlap(pair);
+					} else
+					{
+						needsRemoval = true;
+					}
+				} else
+				{
+					//remove duplicate
+					needsRemoval = true;
+					//should have no algorithm
+					btAssert(!pair.m_algorithm);
+				}
+
+				if (needsRemoval)
+				{
+					m_pairCache->cleanOverlappingPair(pair,dispatcher);
+
+					//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+					//		m_overlappingPairArray.pop_back();
+					pair.m_pProxy0 = 0;
+					pair.m_pProxy1 = 0;
+					m_invalidPair++;
+					gOverlappingPairs--;
+				} 
+
+			}
+
+			///if you don't like to skip the invalid pairs in the array, execute following code:
+#define CLEAN_INVALID_PAIRS 1
+#ifdef CLEAN_INVALID_PAIRS
+
+			//perform a sort, to sort 'invalid' pairs to the end
+			overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+			overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
+			m_invalidPair = 0;
+#endif//CLEAN_INVALID_PAIRS
+
+		}
+	}
+}
+
+
+bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+{
+	btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+	btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+	return aabbOverlap(p0,p1);
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h	(revision 1963)
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMPLE_BROADPHASE_H
+#define SIMPLE_BROADPHASE_H
+
+
+#include "btOverlappingPairCache.h"
+
+
+struct btSimpleBroadphaseProxy : public btBroadphaseProxy
+{
+	int			m_nextFree;
+	
+//	int			m_handleId;
+
+	
+	btSimpleBroadphaseProxy() {};
+
+	btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy)
+	:btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy)
+	{
+		(void)shapeType;
+	}
+	
+	
+	SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
+	SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
+
+	
+
+
+};
+
+///The SimpleBroadphase is just a unit-test for btAxisSweep3, bt32BitAxisSweep3, or btDbvtBroadphase, so use those classes instead.
+///It is a brute force aabb culling broadphase based on O(n^2) aabb checks
+class btSimpleBroadphase : public btBroadphaseInterface
+{
+
+protected:
+
+	int		m_numHandles;						// number of active handles
+	int		m_maxHandles;						// max number of handles
+	
+	btSimpleBroadphaseProxy* m_pHandles;						// handles pool
+
+	void* m_pHandlesRawPtr;
+	int		m_firstFreeHandle;		// free handles list
+	
+	int allocHandle()
+	{
+		btAssert(m_numHandles < m_maxHandles);
+		int freeHandle = m_firstFreeHandle;
+		m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
+		m_numHandles++;
+		return freeHandle;
+	}
+
+	void freeHandle(btSimpleBroadphaseProxy* proxy)
+	{
+		int handle = int(proxy-m_pHandles);
+		btAssert(handle >= 0 && handle < m_maxHandles);
+
+		proxy->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		m_numHandles--;
+	}
+
+	btOverlappingPairCache*	m_pairCache;
+	bool	m_ownsPairCache;
+
+	int	m_invalidPair;
+
+	
+	
+	inline btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy)
+	{
+		btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+	inline const btSimpleBroadphaseProxy*	getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const
+	{
+		const btSimpleBroadphaseProxy* proxy0 = static_cast<const btSimpleBroadphaseProxy*>(proxy);
+		return proxy0;
+	}
+
+
+	void	validate();
+
+protected:
+
+
+	
+
+public:
+	btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0);
+	virtual ~btSimpleBroadphase();
+
+
+		static bool	aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1);
+
+
+	virtual btBroadphaseProxy*	createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy);
+
+	virtual void	calculateOverlappingPairs(btDispatcher* dispatcher);
+
+	virtual void	destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	virtual void	setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher);
+	virtual void	getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+
+	virtual void	rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback);
+		
+	btOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_pairCache;
+	}
+	const btOverlappingPairCache*	getOverlappingPairCache() const
+	{
+		return m_pairCache;
+	}
+
+	bool	testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
+
+
+	///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
+	///will add some transform later
+	virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		aabbMin.setValue(-1e30f,-1e30f,-1e30f);
+		aabbMax.setValue(1e30f,1e30f,1e30f);
+	}
+
+	virtual void	printStats()
+	{
+//		printf("btSimpleBroadphase.h\n");
+//		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+	}
+};
+
+
+
+#endif //SIMPLE_BROADPHASE_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,161 @@
+ADD_LIBRARY(LibBulletCollision
+				BroadphaseCollision/btAxisSweep3.cpp
+				BroadphaseCollision/btAxisSweep3.h
+				BroadphaseCollision/btBroadphaseProxy.cpp
+				BroadphaseCollision/btBroadphaseProxy.h
+				BroadphaseCollision/btCollisionAlgorithm.cpp
+				BroadphaseCollision/btCollisionAlgorithm.h
+				BroadphaseCollision/btDispatcher.cpp
+				BroadphaseCollision/btDispatcher.h
+				BroadphaseCollision/btDbvtBroadphase.cpp
+				BroadphaseCollision/btDbvtBroadphase.h
+				BroadphaseCollision/btDbvt.cpp
+				BroadphaseCollision/btDbvt.h
+				BroadphaseCollision/btMultiSapBroadphase.cpp
+				BroadphaseCollision/btMultiSapBroadphase.h
+				BroadphaseCollision/btOverlappingPairCache.cpp
+				BroadphaseCollision/btOverlappingPairCache.h
+				BroadphaseCollision/btOverlappingPairCallback.h
+				BroadphaseCollision/btQuantizedBvh.cpp
+				BroadphaseCollision/btQuantizedBvh.h
+				BroadphaseCollision/btSimpleBroadphase.cpp
+				BroadphaseCollision/btSimpleBroadphase.h
+				CollisionDispatch/btCollisionDispatcher.cpp
+				CollisionDispatch/btCollisionDispatcher.h
+				CollisionDispatch/btCollisionObject.cpp
+				CollisionDispatch/btCollisionObject.h
+				CollisionDispatch/btCollisionWorld.cpp
+				CollisionDispatch/btCollisionWorld.h
+				CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+				CollisionDispatch/btCompoundCollisionAlgorithm.h
+				CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+				CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+				CollisionDispatch/btDefaultCollisionConfiguration.cpp
+				CollisionDispatch/btDefaultCollisionConfiguration.h
+				CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+				CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+				CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+				CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+				CollisionDispatch/btBoxBoxDetector.cpp
+				CollisionDispatch/btBoxBoxDetector.h
+				CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+				CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+				CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+				CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
+				CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+				CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
+				CollisionDispatch/btConvexConvexAlgorithm.cpp
+				CollisionDispatch/btConvexConvexAlgorithm.h
+				CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+				CollisionDispatch/btEmptyCollisionAlgorithm.h
+				CollisionDispatch/btManifoldResult.cpp
+				CollisionDispatch/btManifoldResult.h
+				CollisionDispatch/btSimulationIslandManager.cpp
+				CollisionDispatch/btSimulationIslandManager.h
+				CollisionDispatch/btUnionFind.cpp
+				CollisionDispatch/btUnionFind.h
+				CollisionDispatch/SphereTriangleDetector.cpp
+				CollisionDispatch/SphereTriangleDetector.h
+				CollisionShapes/btBoxShape.cpp
+				CollisionShapes/btBoxShape.h
+				CollisionShapes/btBvhTriangleMeshShape.cpp
+				CollisionShapes/btBvhTriangleMeshShape.h
+				CollisionShapes/btCapsuleShape.cpp
+				CollisionShapes/btCapsuleShape.h
+				CollisionShapes/btCollisionShape.cpp
+				CollisionShapes/btCollisionShape.h
+				CollisionShapes/btCompoundShape.cpp
+				CollisionShapes/btCompoundShape.h
+				CollisionShapes/btConcaveShape.cpp
+				CollisionShapes/btConcaveShape.h
+				CollisionShapes/btConeShape.cpp
+				CollisionShapes/btConeShape.h
+				CollisionShapes/btConvexHullShape.cpp
+				CollisionShapes/btConvexHullShape.h
+				CollisionShapes/btConvexPointCloudShape.cpp
+				CollisionShapes/btConvexPointCloudShape.h
+				CollisionShapes/btConvexShape.cpp
+				CollisionShapes/btConvexShape.h
+				CollisionShapes/btConvexInternalShape.cpp
+				CollisionShapes/btConvexInternalShape.h
+				CollisionShapes/btConvexTriangleMeshShape.cpp
+				CollisionShapes/btConvexTriangleMeshShape.h
+				CollisionShapes/btCylinderShape.cpp
+				CollisionShapes/btCylinderShape.h
+				CollisionShapes/btEmptyShape.cpp
+				CollisionShapes/btEmptyShape.h
+				CollisionShapes/btHeightfieldTerrainShape.cpp
+				CollisionShapes/btHeightfieldTerrainShape.h
+				CollisionShapes/btMinkowskiSumShape.cpp
+				CollisionShapes/btMinkowskiSumShape.h
+				CollisionShapes/btMaterial.h
+				CollisionShapes/btMultimaterialTriangleMeshShape.cpp
+				CollisionShapes/btMultimaterialTriangleMeshShape.h
+				CollisionShapes/btMultiSphereShape.cpp
+				CollisionShapes/btMultiSphereShape.h
+				CollisionShapes/btOptimizedBvh.cpp
+				CollisionShapes/btOptimizedBvh.h
+				CollisionShapes/btPolyhedralConvexShape.cpp
+				CollisionShapes/btPolyhedralConvexShape.h
+				CollisionShapes/btScaledBvhTriangleMeshShape.cpp
+				CollisionShapes/btScaledBvhTriangleMeshShape.h
+				CollisionShapes/btTetrahedronShape.cpp
+				CollisionShapes/btTetrahedronShape.h
+				CollisionShapes/btSphereShape.cpp
+				CollisionShapes/btSphereShape.h
+				CollisionShapes/btShapeHull.h
+				CollisionShapes/btShapeHull.cpp
+				CollisionShapes/btStaticPlaneShape.cpp
+				CollisionShapes/btStaticPlaneShape.h
+				CollisionShapes/btStridingMeshInterface.cpp
+				CollisionShapes/btStridingMeshInterface.h
+				CollisionShapes/btTriangleCallback.cpp
+				CollisionShapes/btTriangleCallback.h
+				CollisionShapes/btTriangleBuffer.cpp
+				CollisionShapes/btTriangleBuffer.h
+				CollisionShapes/btTriangleIndexVertexArray.cpp
+				CollisionShapes/btTriangleIndexVertexArray.h
+				CollisionShapes/btTriangleIndexVertexMaterialArray.h
+				CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
+				CollisionShapes/btTriangleMesh.cpp
+				CollisionShapes/btTriangleMesh.h
+				CollisionShapes/btTriangleMeshShape.cpp
+				CollisionShapes/btTriangleMeshShape.h
+				CollisionShapes/btUniformScalingShape.cpp
+				CollisionShapes/btUniformScalingShape.h
+				Gimpact/btContactProcessing.cpp
+				Gimpact/btGImpactShape.cpp
+				Gimpact/gim_contact.cpp
+				Gimpact/btGImpactBvh.cpp
+				Gimpact/btGenericPoolAllocator.cpp
+				Gimpact/gim_memory.cpp
+				Gimpact/btGImpactCollisionAlgorithm.cpp
+				Gimpact/btTriangleShapeEx.cpp
+				Gimpact/gim_tri_collision.cpp
+				Gimpact/btGImpactQuantizedBvh.cpp
+				Gimpact/gim_box_set.cpp
+				NarrowPhaseCollision/btContinuousConvexCollision.cpp
+				NarrowPhaseCollision/btContinuousConvexCollision.h
+				NarrowPhaseCollision/btGjkEpa.cpp
+				NarrowPhaseCollision/btGjkEpa.h
+				NarrowPhaseCollision/btGjkEpa2.cpp
+				NarrowPhaseCollision/btGjkEpa2.h
+				NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+				NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
+				NarrowPhaseCollision/btConvexCast.cpp
+				NarrowPhaseCollision/btConvexCast.h
+				NarrowPhaseCollision/btGjkConvexCast.cpp
+				NarrowPhaseCollision/btGjkConvexCast.h
+				NarrowPhaseCollision/btGjkPairDetector.cpp
+				NarrowPhaseCollision/btGjkPairDetector.h
+				NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+				NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
+				NarrowPhaseCollision/btPersistentManifold.cpp
+				NarrowPhaseCollision/btPersistentManifold.h
+				NarrowPhaseCollision/btRaycastCallback.cpp
+				NarrowPhaseCollision/btRaycastCallback.h
+				NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+				NarrowPhaseCollision/btSubSimplexConvexCast.h
+				NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+				NarrowPhaseCollision/btVoronoiSimplexSolver.h
+)
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp	(revision 1963)
@@ -0,0 +1,211 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
+:m_sphere(sphere),
+m_triangle(triangle)
+{
+
+}
+
+void	SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+
+	(void)debugDraw;
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+
+	btVector3 point,normal;
+	btScalar timeOfImpact = btScalar(1.);
+	btScalar depth = btScalar(0.);
+//	output.m_distance = btScalar(1e30);
+	//move sphere into triangle space
+	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+
+	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
+	{
+		if (swapResults)
+		{
+			btVector3 normalOnB = transformB.getBasis()*normal;
+			btVector3 normalOnA = -normalOnB;
+			btVector3 pointOnA = transformB*point+normalOnB*depth;
+			output.addContactPoint(normalOnA,pointOnA,depth);
+		} else
+		{
+			output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+		}
+	}
+
+}
+
+#define MAX_OVERLAP btScalar(0.)
+
+
+
+// See also geometrictools.com
+// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
+
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+	btVector3 diff = p - from;
+	btVector3 v = to - from;
+	btScalar t = v.dot(diff);
+	
+	if (t > 0) {
+		btScalar dotVV = v.dot(v);
+		if (t < dotVV) {
+			t /= dotVV;
+			diff -= t*v;
+		} else {
+			t = 1;
+			diff -= v;
+		}
+	} else
+		t = 0;
+
+	nearest = from + t*v;
+	return diff.dot(diff);	
+}
+
+bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
+	btVector3 lp(p);
+	btVector3 lnormal(normal);
+	
+	return pointInTriangle(vertices, lnormal, &lp);
+}
+
+///combined discrete/continuous sphere-triangle
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
+{
+
+	const btVector3* vertices = &m_triangle->getVertexPtr(0);
+	const btVector3& c = sphereCenter;
+	btScalar r = m_sphere->getRadius();
+
+	btVector3 delta (0,0,0);
+
+	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+	normal.normalize();
+	btVector3 p1ToCentre = c - vertices[0];
+	btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+	if (distanceFromPlane < btScalar(0.))
+	{
+		//triangle facing the other way
+	
+		distanceFromPlane *= btScalar(-1.);
+		normal *= btScalar(-1.);
+	}
+
+	///todo: move this gContactBreakingThreshold into a proper structure
+	extern btScalar gContactBreakingThreshold;
+
+	btScalar contactMargin = gContactBreakingThreshold;
+	bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+	bool isInsideShellPlane = distanceFromPlane < r;
+	
+	btScalar deltaDotNormal = delta.dot(normal);
+	if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
+		return false;
+
+	// Check for contact / intersection
+	bool hasContact = false;
+	btVector3 contactPoint;
+	if (isInsideContactPlane) {
+		if (facecontains(c,vertices,normal)) {
+			// Inside the contact wedge - touches a point on the shell plane
+			hasContact = true;
+			contactPoint = c - normal*distanceFromPlane;
+		} else {
+			// Could be inside one of the contact capsules
+			btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+			btVector3 nearestOnEdge;
+			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+				
+				btPoint3 pa;
+				btPoint3 pb;
+				
+				m_triangle->getEdge(i,pa,pb);
+
+				btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
+				if (distanceSqr < contactCapsuleRadiusSqr) {
+					// Yep, we're inside a capsule
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+				}
+				
+			}
+		}
+	}
+
+	if (hasContact) {
+		btVector3 contactToCentre = c - contactPoint;
+		btScalar distanceSqr = contactToCentre.length2();
+		if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
+			btScalar distance = btSqrt(distanceSqr);
+			resultNormal = contactToCentre;
+			resultNormal.normalize();
+			point = contactPoint;
+			depth = -(r-distance);
+			return true;
+		}
+
+		if (delta.dot(contactToCentre) >= btScalar(0.0)) 
+			return false;
+		
+		// Moving towards the contact point -> collision
+		point = contactPoint;
+		timeOfImpact = btScalar(0.0);
+		return true;
+	}
+	
+	return false;
+}
+
+
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+{
+	const btVector3* p1 = &vertices[0];
+	const btVector3* p2 = &vertices[1];
+	const btVector3* p3 = &vertices[2];
+
+	btVector3 edge1( *p2 - *p1 );
+	btVector3 edge2( *p3 - *p2 );
+	btVector3 edge3( *p1 - *p3 );
+
+	btVector3 p1_to_p( *p - *p1 );
+	btVector3 p2_to_p( *p - *p2 );
+	btVector3 p3_to_p( *p - *p3 );
+
+	btVector3 edge1_normal( edge1.cross(normal));
+	btVector3 edge2_normal( edge2.cross(normal));
+	btVector3 edge3_normal( edge3.cross(normal));
+	
+	btScalar r1, r2, r3;
+	r1 = edge1_normal.dot( p1_to_p );
+	r2 = edge2_normal.dot( p2_to_p );
+	r3 = edge3_normal.dot( p3_to_p );
+	if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
+	     ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+		return true;
+	return false;
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h	(revision 1963)
@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_DETECTOR_H
+#define SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/btPoint3.h"
+
+
+class btSphereShape;
+class btTriangleShape;
+
+
+
+/// sphere-triangle to match the btDiscreteCollisionDetectorInterface
+struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface
+{
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+	SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle);
+
+	virtual ~SphereTriangleDetector() {};
+
+private:
+
+	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact);
+	bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
+	bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);
+
+	btSphereShape* m_sphere;
+	btTriangleShape* m_triangle;
+
+	
+};
+#endif //SPHERE_TRIANGLE_DETECTOR_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "btBoxBoxDetector.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
+		m_ownManifold = true;
+	}
+}
+
+btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject*	col0 = body0;
+	btCollisionObject*	col1 = body1;
+	btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
+	btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
+
+
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+#ifndef USE_PERSISTENT_CONTACTS	
+	m_manifoldPtr->clearManifold();
+#endif //USE_PERSISTENT_CONTACTS
+
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = 1e30f;
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+
+	btBoxBoxDetector detector(box0,box1);
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+#ifdef USE_PERSISTENT_CONTACTS
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+#endif //USE_PERSISTENT_CONTACTS
+
+}
+
+btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	//not yet
+	return 1.f;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX_BOX__COLLISION_ALGORITHM_H
+#define BOX_BOX__COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBoxBoxCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btBoxBoxCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //BOX_BOX__COLLISION_ALGORITHM_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp	(revision 1963)
@@ -0,0 +1,683 @@
+
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///ODE box-box collision detection is adapted to work with Bullet
+
+#include "btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+
+#include <float.h>
+#include <string.h>
+
+btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
+: m_box1(box1),
+m_box2(box2)
+{
+
+}
+
+
+// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
+// generate contact points. this returns 0 if there is no contact otherwise
+// it returns the number of contacts generated.
+// `normal' returns the contact normal.
+// `depth' returns the maximum penetration depth along that normal.
+// `return_code' returns a number indicating the type of contact that was
+// detected:
+//        1,2,3 = box 2 intersects with a face of box 1
+//        4,5,6 = box 1 intersects with a face of box 2
+//        7..15 = edge-edge contact
+// `maxc' is the maximum number of contacts allowed to be generated, i.e.
+// the size of the `contact' array.
+// `contact' and `skip' are the contact array information provided to the
+// collision functions. this function only fills in the position and depth
+// fields.
+struct dContactGeom;
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+#define dInfinity FLT_MAX
+
+
+/*PURE_INLINE btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
+PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
+PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
+*/
+static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
+static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
+static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
+static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
+#define dMULTIPLYOP1_331(A,op,B,C) \
+{\
+  (A)[0] op dDOT41((B),(C)); \
+  (A)[1] op dDOT41((B+1),(C)); \
+  (A)[2] op dDOT41((B+2),(C)); \
+}
+
+#define dMULTIPLYOP0_331(A,op,B,C) \
+{ \
+  (A)[0] op dDOT((B),(C)); \
+  (A)[1] op dDOT((B+4),(C)); \
+  (A)[2] op dDOT((B+8),(C)); \
+} 
+
+#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+
+typedef btScalar dMatrix3[4*3];
+
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+			   const btVector3& pb, const btVector3& ub,
+			   btScalar *alpha, btScalar *beta);
+void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
+			   const btVector3& pb, const btVector3& ub,
+			   btScalar *alpha, btScalar *beta)
+{
+  btVector3 p;
+  p[0] = pb[0] - pa[0];
+  p[1] = pb[1] - pa[1];
+  p[2] = pb[2] - pa[2];
+  btScalar uaub = dDOT(ua,ub);
+  btScalar q1 =  dDOT(ua,p);
+  btScalar q2 = -dDOT(ub,p);
+  btScalar d = 1-uaub*uaub;
+  if (d <= btScalar(0.0001f)) {
+    // @@@ this needs to be made more robust
+    *alpha = 0;
+    *beta  = 0;
+  }
+  else {
+    d = 1.f/d;
+    *alpha = (q1 + uaub*q2)*d;
+    *beta  = (uaub*q1 + q2)*d;
+  }
+}
+
+
+
+// find all the intersection points between the 2D rectangle with vertices
+// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
+// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
+//
+// the intersection points are returned as x,y pairs in the 'ret' array.
+// the number of intersection points is returned by the function (this will
+// be in the range 0 to 8).
+
+static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
+{
+  // q (and r) contain nq (and nr) coordinate points for the current (and
+  // chopped) polygons
+  int nq=4,nr=0;
+  btScalar buffer[16];
+  btScalar *q = p;
+  btScalar *r = ret;
+  for (int dir=0; dir <= 1; dir++) {
+    // direction notation: xy[0] = x axis, xy[1] = y axis
+    for (int sign=-1; sign <= 1; sign += 2) {
+      // chop q along the line xy[dir] = sign*h[dir]
+      btScalar *pq = q;
+      btScalar *pr = r;
+      nr = 0;
+      for (int i=nq; i > 0; i--) {
+	// go through all points in q and all lines between adjacent points
+	if (sign*pq[dir] < h[dir]) {
+	  // this point is inside the chopping line
+	  pr[0] = pq[0];
+	  pr[1] = pq[1];
+	  pr += 2;
+	  nr++;
+	  if (nr & 8) {
+	    q = r;
+	    goto done;
+	  }
+	}
+	btScalar *nextq = (i > 1) ? pq+2 : q;
+	if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
+	  // this line crosses the chopping line
+	  pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+	    (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+	  pr[dir] = sign*h[dir];
+	  pr += 2;
+	  nr++;
+	  if (nr & 8) {
+	    q = r;
+	    goto done;
+	  }
+	}
+	pq += 2;
+      }
+      q = r;
+      r = (q==ret) ? buffer : ret;
+      nq = nr;
+    }
+  }
+ done:
+  if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
+  return nr;
+}
+
+
+#define M__PI 3.14159265f
+
+// given n points in the plane (array p, of size 2*n), generate m points that
+// best represent the whole set. the definition of 'best' here is not
+// predetermined - the idea is to select points that give good box-box
+// collision detection behavior. the chosen point indexes are returned in the
+// array iret (of size m). 'i0' is always the first entry in the array.
+// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
+// in the range [0..n-1].
+
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
+void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
+{
+  // compute the centroid of the polygon in cx,cy
+  int i,j;
+  btScalar a,cx,cy,q;
+  if (n==1) {
+    cx = p[0];
+    cy = p[1];
+  }
+  else if (n==2) {
+    cx = btScalar(0.5)*(p[0] + p[2]);
+    cy = btScalar(0.5)*(p[1] + p[3]);
+  }
+  else {
+    a = 0;
+    cx = 0;
+    cy = 0;
+    for (i=0; i<(n-1); i++) {
+      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
+      a += q;
+      cx += q*(p[i*2]+p[i*2+2]);
+      cy += q*(p[i*2+1]+p[i*2+3]);
+    }
+    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
+    a = 1.f/(btScalar(3.0)*(a+q));
+    cx = a*(cx + q*(p[n*2-2]+p[0]));
+    cy = a*(cy + q*(p[n*2-1]+p[1]));
+  }
+
+  // compute the angle of each point w.r.t. the centroid
+  btScalar A[8];
+  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
+
+  // search for points that have angles closest to A[i0] + i*(2*pi/m).
+  int avail[8];
+  for (i=0; i<n; i++) avail[i] = 1;
+  avail[i0] = 0;
+  iret[0] = i0;
+  iret++;
+  for (j=1; j<m; j++) {
+    a = btScalar(j)*(2*M__PI/m) + A[i0];
+    if (a > M__PI) a -= 2*M__PI;
+    btScalar maxdiff=1e9,diff;
+
+    *iret = i0;			// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+
+    for (i=0; i<n; i++) {
+      if (avail[i]) {
+	diff = btFabs (A[i]-a);
+	if (diff > M__PI) diff = 2*M__PI - diff;
+	if (diff < maxdiff) {
+	  maxdiff = diff;
+	  *iret = i;
+	}
+      }
+    }
+#if defined(DEBUG) || defined (_DEBUG)
+    btAssert (*iret != i0);	// ensure iret got set
+#endif
+    avail[*iret] = 0;
+    iret++;
+  }
+}
+
+
+
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+	     const btVector3& side1, const btVector3& p2,
+	     const dMatrix3 R2, const btVector3& side2,
+	     btVector3& normal, btScalar *depth, int *return_code,
+		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
+int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
+	     const btVector3& side1, const btVector3& p2,
+	     const dMatrix3 R2, const btVector3& side2,
+	     btVector3& normal, btScalar *depth, int *return_code,
+		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
+{
+  const btScalar fudge_factor = btScalar(1.05);
+  btVector3 p,pp,normalC;
+  const btScalar *normalR = 0;
+  btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
+    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
+  int i,j,invert_normal,code;
+
+  // get vector from centers of box 1 to box 2, relative to box 1
+  p = p2 - p1;
+  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
+
+  // get side lengths / 2
+  A[0] = side1[0]*btScalar(0.5);
+  A[1] = side1[1]*btScalar(0.5);
+  A[2] = side1[2]*btScalar(0.5);
+  B[0] = side2[0]*btScalar(0.5);
+  B[1] = side2[1]*btScalar(0.5);
+  B[2] = side2[2]*btScalar(0.5);
+
+  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+
+  Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
+  Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
+  Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
+
+  // for all 15 possible separating axes:
+  //   * see if the axis separates the boxes. if so, return 0.
+  //   * find the depth of the penetration along the separating axis (s2)
+  //   * if this is the largest depth so far, record it.
+  // the normal vector will be set to the separating axis with the smallest
+  // depth. note: normalR is set to point to a column of R1 or R2 if that is
+  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+  // set to a vector relative to body 1. invert_normal is 1 if the sign of
+  // the normal should be flipped.
+
+#define TST(expr1,expr2,norm,cc) \
+  s2 = btFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1) < 0); \
+    code = (cc); \
+  }
+
+  s = -dInfinity;
+  invert_normal = 0;
+  code = 0;
+
+  // separating axis = u1,u2,u3
+  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
+  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
+  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
+
+  // separating axis = v1,v2,v3
+  TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
+  TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
+  TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
+
+  // note: cross product axes need to be scaled when s is computed.
+  // normal (n1,n2,n3) is relative to box 1.
+#undef TST
+#define TST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = btFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+  if (l > 0) { \
+    s2 /= l; \
+    if (s2*fudge_factor > s) { \
+      s = s2; \
+      normalR = 0; \
+      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
+      invert_normal = ((expr1) < 0); \
+      code = (cc); \
+    } \
+  }
+
+  // separating axis = u1 x (v1,v2,v3)
+  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
+  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
+  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
+
+  // separating axis = u2 x (v1,v2,v3)
+  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
+  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
+  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
+
+  // separating axis = u3 x (v1,v2,v3)
+  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
+  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
+  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+
+#undef TST
+
+  if (!code) return 0;
+
+  // if we get to this point, the boxes interpenetrate. compute the normal
+  // in global coordinates.
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+    dMULTIPLY0_331 (normal,R1,normalC);
+  }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+  *depth = -s;
+
+  // compute contact point(s)
+
+  if (code > 6) {
+    // an edge from box 1 touches an edge from box 2.
+    // find a point pa on the intersecting edge of box 1
+    btVector3 pa;
+    btScalar sign;
+    for (i=0; i<3; i++) pa[i] = p1[i];
+    for (j=0; j<3; j++) {
+      sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
+      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
+    }
+
+    // find a point pb on the intersecting edge of box 2
+    btVector3 pb;
+    for (i=0; i<3; i++) pb[i] = p2[i];
+    for (j=0; j<3; j++) {
+      sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
+      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
+    }
+
+    btScalar alpha,beta;
+    btVector3 ua,ub;
+    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
+    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
+
+    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
+    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+
+	{
+		
+		//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
+		//contact[0].depth = *depth;
+		btVector3 pointInWorld;
+
+#ifdef USE_CENTER_POINT
+	    for (i=0; i<3; i++) 
+			pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
+		output.addContactPoint(-normal,pointInWorld,-*depth);
+#else
+		output.addContactPoint(-normal,pb,-*depth);
+#endif //
+		*return_code = code;
+	}
+    return 1;
+  }
+
+  // okay, we have a face-something intersection (because the separating
+  // axis is perpendicular to a face). define face 'a' to be the reference
+  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+  // the incident face (the closest face of the other box).
+
+  const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
+  if (code <= 3) {
+    Ra = R1;
+    Rb = R2;
+    pa = p1;
+    pb = p2;
+    Sa = A;
+    Sb = B;
+  }
+  else {
+    Ra = R2;
+    Rb = R1;
+    pa = p2;
+    pb = p1;
+    Sa = B;
+    Sb = A;
+  }
+
+  // nr = normal vector of reference face dotted with axes of incident box.
+  // anr = absolute values of nr.
+  btVector3 normal2,nr,anr;
+  if (code <= 3) {
+    normal2[0] = normal[0];
+    normal2[1] = normal[1];
+    normal2[2] = normal[2];
+  }
+  else {
+    normal2[0] = -normal[0];
+    normal2[1] = -normal[1];
+    normal2[2] = -normal[2];
+  }
+  dMULTIPLY1_331 (nr,Rb,normal2);
+  anr[0] = btFabs (nr[0]);
+  anr[1] = btFabs (nr[1]);
+  anr[2] = btFabs (nr[2]);
+
+  // find the largest compontent of anr: this corresponds to the normal
+  // for the indident face. the other axis numbers of the indicent face
+  // are stored in a1,a2.
+  int lanr,a1,a2;
+  if (anr[1] > anr[0]) {
+    if (anr[1] > anr[2]) {
+      a1 = 0;
+      lanr = 1;
+      a2 = 2;
+    }
+    else {
+      a1 = 0;
+      a2 = 1;
+      lanr = 2;
+    }
+  }
+  else {
+    if (anr[0] > anr[2]) {
+      lanr = 0;
+      a1 = 1;
+      a2 = 2;
+    }
+    else {
+      a1 = 0;
+      a2 = 1;
+      lanr = 2;
+    }
+  }
+
+  // compute center point of incident face, in reference-face coordinates
+  btVector3 center;
+  if (nr[lanr] < 0) {
+    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
+  }
+  else {
+    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
+  }
+
+  // find the normal and non-normal axis numbers of the reference box
+  int codeN,code1,code2;
+  if (code <= 3) codeN = code-1; else codeN = code-4;
+  if (codeN==0) {
+    code1 = 1;
+    code2 = 2;
+  }
+  else if (codeN==1) {
+    code1 = 0;
+    code2 = 2;
+  }
+  else {
+    code1 = 0;
+    code2 = 1;
+  }
+
+  // find the four corners of the incident face, in reference-face coordinates
+  btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
+  btScalar c1,c2,m11,m12,m21,m22;
+  c1 = dDOT14 (center,Ra+code1);
+  c2 = dDOT14 (center,Ra+code2);
+  // optimize this? - we have already computed this data above, but it is not
+  // stored in an easy-to-index format. for now it's quicker just to recompute
+  // the four dot products.
+  m11 = dDOT44 (Ra+code1,Rb+a1);
+  m12 = dDOT44 (Ra+code1,Rb+a2);
+  m21 = dDOT44 (Ra+code2,Rb+a1);
+  m22 = dDOT44 (Ra+code2,Rb+a2);
+  {
+    btScalar k1 = m11*Sb[a1];
+    btScalar k2 = m21*Sb[a1];
+    btScalar k3 = m12*Sb[a2];
+    btScalar k4 = m22*Sb[a2];
+    quad[0] = c1 - k1 - k3;
+    quad[1] = c2 - k2 - k4;
+    quad[2] = c1 - k1 + k3;
+    quad[3] = c2 - k2 + k4;
+    quad[4] = c1 + k1 + k3;
+    quad[5] = c2 + k2 + k4;
+    quad[6] = c1 + k1 - k3;
+    quad[7] = c2 + k2 - k4;
+  }
+
+  // find the size of the reference face
+  btScalar rect[2];
+  rect[0] = Sa[code1];
+  rect[1] = Sa[code2];
+
+  // intersect the incident and reference faces
+  btScalar ret[16];
+  int n = intersectRectQuad2 (rect,quad,ret);
+  if (n < 1) return 0;		// this should never happen
+
+  // convert the intersection points into reference-face coordinates,
+  // and compute the contact position and depth for each point. only keep
+  // those points that have a positive (penetrating) depth. delete points in
+  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
+  btScalar point[3*8];		// penetrating contact points
+  btScalar dep[8];			// depths for those points
+  btScalar det1 = 1.f/(m11*m22 - m12*m21);
+  m11 *= det1;
+  m12 *= det1;
+  m21 *= det1;
+  m22 *= det1;
+  int cnum = 0;			// number of penetrating contact points found
+  for (j=0; j < n; j++) {
+    btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+    btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+    for (i=0; i<3; i++) point[cnum*3+i] =
+			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
+    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
+    if (dep[cnum] >= 0) {
+      ret[cnum*2] = ret[j*2];
+      ret[cnum*2+1] = ret[j*2+1];
+      cnum++;
+    }
+  }
+  if (cnum < 1) return 0;	// this should never happen
+
+  // we can't generate more contacts than we actually have
+  if (maxc > cnum) maxc = cnum;
+  if (maxc < 1) maxc = 1;
+
+  if (cnum <= maxc) {
+    // we have less contacts than we need, so we use them all
+    for (j=0; j < cnum; j++) {
+
+		//AddContactPoint...
+
+		//dContactGeom *con = CONTACT(contact,skip*j);
+      //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
+      //con->depth = dep[j];
+
+		btVector3 pointInWorld;
+		for (i=0; i<3; i++) 
+			pointInWorld[i] = point[j*3+i] + pa[i];
+		output.addContactPoint(-normal,pointInWorld,-dep[j]);
+
+    }
+  }
+  else {
+    // we have more contacts than are wanted, some of them must be culled.
+    // find the deepest point, it is always the first contact.
+    int i1 = 0;
+    btScalar maxdepth = dep[0];
+    for (i=1; i<cnum; i++) {
+      if (dep[i] > maxdepth) {
+	maxdepth = dep[i];
+	i1 = i;
+      }
+    }
+
+    int iret[8];
+    cullPoints2 (cnum,ret,maxc,i1,iret);
+
+    for (j=0; j < maxc; j++) {
+//      dContactGeom *con = CONTACT(contact,skip*j);
+  //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+    //  con->depth = dep[iret[j]];
+
+		btVector3 posInWorld;
+		for (i=0; i<3; i++) 
+			posInWorld[i] = point[iret[j]*3+i] + pa[i];
+		output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+    }
+    cnum = maxc;
+  }
+
+  *return_code = code;
+  return cnum;
+}
+
+void	btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
+{
+	
+	const btTransform& transformA = input.m_transformA;
+	const btTransform& transformB = input.m_transformB;
+	
+	int skip = 0;
+	dContactGeom *contact = 0;
+
+	dMatrix3 R1;
+	dMatrix3 R2;
+
+	for (int j=0;j<3;j++)
+	{
+		R1[0+4*j] = transformA.getBasis()[j].x();
+		R2[0+4*j] = transformB.getBasis()[j].x();
+
+		R1[1+4*j] = transformA.getBasis()[j].y();
+		R2[1+4*j] = transformB.getBasis()[j].y();
+
+
+		R1[2+4*j] = transformA.getBasis()[j].z();
+		R2[2+4*j] = transformB.getBasis()[j].z();
+
+	}
+
+	
+
+	btVector3 normal;
+	btScalar depth;
+	int return_code;
+	int maxc = 4;
+
+
+	dBoxBox2 (transformA.getOrigin(), 
+	R1,
+	2.f*m_box1->getHalfExtentsWithMargin(),
+	transformB.getOrigin(),
+	R2, 
+	2.f*m_box2->getHalfExtentsWithMargin(),
+	normal, &depth, &return_code,
+	maxc, contact, skip,
+	output
+	);
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h	(revision 1963)
@@ -0,0 +1,44 @@
+/*
+ * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BOX_BOX_DETECTOR_H
+#define BOX_BOX_DETECTOR_H
+
+
+class btBoxShape;
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+
+/// btBoxBoxDetector wraps the ODE box-box collision detector
+/// re-distributed under the Zlib license with permission from Russell L. Smith
+struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
+{
+	btBoxShape* m_box1;
+	btBoxShape* m_box2;
+
+public:
+
+	btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
+
+	virtual ~btBoxBoxDetector() {};
+
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+};
+
+#endif //BT_BOX_BOX_DETECTOR_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h	(revision 1963)
@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_COLLISION_CONFIGURATION
+#define BT_COLLISION_CONFIGURATION
+struct btCollisionAlgorithmCreateFunc;
+
+class btStackAlloc;
+class btPoolAllocator;
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator size, default collision algorithms and persistent manifold pool size
+///todo: describe the meaning
+class	btCollisionConfiguration
+{
+
+public:
+
+	virtual ~btCollisionConfiguration()
+	{
+	}
+
+	///memory pools
+	virtual btPoolAllocator* getPersistentManifoldPool() = 0;
+
+	virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
+
+	virtual btStackAlloc*	getStackAllocator() = 0;
+
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
+
+};
+
+#endif //BT_COLLISION_CONFIGURATION
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h	(revision 1963)
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_CREATE_FUNC
+#define COLLISION_CREATE_FUNC
+
+#include "LinearMath/btAlignedObjectArray.h"
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+class btCollisionAlgorithm;
+class btCollisionObject;
+
+struct btCollisionAlgorithmConstructionInfo;
+
+///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm
+struct btCollisionAlgorithmCreateFunc
+{
+	bool m_swapped;
+	
+	btCollisionAlgorithmCreateFunc()
+		:m_swapped(false)
+	{
+	}
+	virtual ~btCollisionAlgorithmCreateFunc(){};
+
+	virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , btCollisionObject* body0,btCollisionObject* body1)
+	{
+		
+		(void)body0;
+		(void)body1;
+		return 0;
+	}
+};
+#endif //COLLISION_CREATE_FUNC
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp	(revision 1963)
@@ -0,0 +1,298 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btCollisionDispatcher.h"
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btPoolAllocator.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+
+int gNumManifold = 0;
+
+#ifdef BT_DEBUG
+#include <stdio.h>
+#endif
+
+
+btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
+	m_count(0),
+	m_useIslands(true),
+	m_staticWarningReported(false),
+	m_collisionConfiguration(collisionConfiguration)
+{
+	int i;
+
+	setNearCallback(defaultNearCallback);
+	
+	m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
+
+	m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
+
+	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	{
+		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		{
+			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+			assert(m_doubleDispatch[i][j]);
+		}
+	}
+	
+	
+};
+
+
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+}
+
+btCollisionDispatcher::~btCollisionDispatcher()
+{
+}
+
+btPersistentManifold*	btCollisionDispatcher::getNewManifold(void* b0,void* b1) 
+{ 
+	gNumManifold++;
+	
+	//btAssert(gNumManifold < 65535);
+	
+
+	btCollisionObject* body0 = (btCollisionObject*)b0;
+	btCollisionObject* body1 = (btCollisionObject*)b1;
+	
+	void* mem = 0;
+	
+	if (m_persistentManifoldPoolAllocator->getFreeCount())
+	{
+		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	} else
+	{
+		mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
+
+	}
+	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
+	manifold->m_index1a = m_manifoldsPtr.size();
+	m_manifoldsPtr.push_back(manifold);
+
+	return manifold;
+}
+
+void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
+{
+	manifold->clearManifold();
+}
+
+	
+void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
+{
+	
+	gNumManifold--;
+
+	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
+	clearManifold(manifold);
+
+	int findIndex = manifold->m_index1a;
+	btAssert(findIndex < m_manifoldsPtr.size());
+	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
+	m_manifoldsPtr.pop_back();
+
+	manifold->~btPersistentManifold();
+	if (m_persistentManifoldPoolAllocator->validPtr(manifold))
+	{
+		m_persistentManifoldPoolAllocator->freeMemory(manifold);
+	} else
+	{
+		btAlignedFree(manifold);
+	}
+	
+}
+
+	
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
+{
+	
+	btCollisionAlgorithmConstructionInfo ci;
+
+	ci.m_dispatcher1 = this;
+	ci.m_manifold = sharedManifold;
+	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1);
+
+	return algo;
+}
+
+
+
+
+
+bool	btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
+{
+	//here you can do filtering
+	bool hasResponse = 
+		(body0->hasContactResponse() && body1->hasContactResponse());
+	//no response between two static/kinematic bodies:
+	hasResponse = hasResponse &&
+		((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+	return hasResponse;
+}
+
+bool	btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
+{
+	assert(body0);
+	assert(body1);
+
+	bool needsCollision = true;
+
+#ifdef BT_DEBUG
+	if (!m_staticWarningReported)
+	{
+		//broadphase filtering already deals with this
+		if ((body0->isStaticObject() || body0->isKinematicObject()) &&
+			(body1->isStaticObject() || body1->isKinematicObject()))
+		{
+			m_staticWarningReported = true;
+			printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+		}
+	}
+#endif //BT_DEBUG
+
+	if ((!body0->isActive()) && (!body1->isActive()))
+		needsCollision = false;
+	else if (!body0->checkCollideWith(body1))
+		needsCollision = false;
+	
+	return needsCollision ;
+
+}
+
+
+
+///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
+///this is useful for the collision dispatcher.
+class btCollisionPairCallback : public btOverlapCallback
+{
+	const btDispatcherInfo& m_dispatchInfo;
+	btCollisionDispatcher*	m_dispatcher;
+
+public:
+
+	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher*	dispatcher)
+	:m_dispatchInfo(dispatchInfo),
+	m_dispatcher(dispatcher)
+	{
+	}
+
+	/*btCollisionPairCallback& operator=(btCollisionPairCallback& other)
+	{
+		m_dispatchInfo = other.m_dispatchInfo;
+		m_dispatcher = other.m_dispatcher;
+		return *this;
+	}
+	*/
+
+
+	virtual ~btCollisionPairCallback() {}
+
+
+	virtual bool	processOverlap(btBroadphasePair& pair)
+	{
+		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
+
+		return false;
+	}
+};
+
+
+
+void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
+{
+	//m_blockedForChanges = true;
+
+	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
+
+	pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher);
+
+	//m_blockedForChanges = false;
+
+}
+
+
+
+
+//by default, Bullet will use this near callback
+void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
+{
+		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+		if (dispatcher.needsCollision(colObj0,colObj1))
+		{
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!collisionPair.m_algorithm)
+			{
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
+			}
+
+			if (collisionPair.m_algorithm)
+			{
+				btManifoldResult contactPointResult(colObj0,colObj1);
+				
+				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+				{
+					//discrete collision detection query
+					collisionPair.m_algorithm->processCollision(colObj0,colObj1,dispatchInfo,&contactPointResult);
+				} else
+				{
+					//continuous collision detection query, time of impact (toi)
+					btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
+					if (dispatchInfo.m_timeOfImpact > toi)
+						dispatchInfo.m_timeOfImpact = toi;
+
+				}
+			}
+		}
+
+}
+
+
+void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
+{
+	if (m_collisionAlgorithmPoolAllocator->getFreeCount())
+	{
+		return m_collisionAlgorithmPoolAllocator->allocate(size);
+	}
+	
+	//warn user for overflow?
+	return	btAlignedAlloc(static_cast<size_t>(size), 16);
+}
+
+void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
+{
+	if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
+	{
+		m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
+	} else
+	{
+		btAlignedFree(ptr);
+	}
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h	(revision 1963)
@@ -0,0 +1,147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION__DISPATCHER_H
+#define COLLISION__DISPATCHER_H
+
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIDebugDraw;
+class btOverlappingPairCache;
+class btPoolAllocator;
+class btCollisionConfiguration;
+
+#include "btCollisionCreateFunc.h"
+
+#define USE_DISPATCH_REGISTRY_ARRAY 1
+
+class btCollisionDispatcher;
+///user can override this nearcallback for collision filtering and more finegrained control over collision detection
+typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+
+///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+///Time of Impact, Closest Points and Penetration Depth.
+class btCollisionDispatcher : public btDispatcher
+{
+	int m_count;
+	
+	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
+
+	bool m_useIslands;
+
+	bool	m_staticWarningReported;
+	
+	btManifoldResult	m_defaultManifoldResult;
+
+	btNearCallback		m_nearCallback;
+	
+	btPoolAllocator*	m_collisionAlgorithmPoolAllocator;
+
+	btPoolAllocator*	m_persistentManifoldPoolAllocator;
+
+	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	
+
+	btCollisionConfiguration*	m_collisionConfiguration;
+
+
+public:
+
+	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
+	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
+
+	int	getNumManifolds() const
+	{ 
+		return int( m_manifoldsPtr.size());
+	}
+
+	btPersistentManifold**	getInternalManifoldPointer()
+	{
+		return &m_manifoldsPtr[0];
+	}
+
+	 btPersistentManifold* getManifoldByIndexInternal(int index)
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	 const btPersistentManifold* getManifoldByIndexInternal(int index) const
+	{
+		return m_manifoldsPtr[index];
+	}
+
+	btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btCollisionDispatcher();
+
+	virtual btPersistentManifold*	getNewManifold(void* b0,void* b1);
+	
+	virtual void releaseManifold(btPersistentManifold* manifold);
+
+
+	virtual void clearManifold(btPersistentManifold* manifold);
+
+			
+	btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
+		
+	virtual bool	needsCollision(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual bool	needsResponse(btCollisionObject* body0,btCollisionObject* body1);
+	
+	virtual void	dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
+
+	void	setNearCallback(btNearCallback	nearCallback)
+	{
+		m_nearCallback = nearCallback; 
+	}
+
+	btNearCallback	getNearCallback() const
+	{
+		return m_nearCallback;
+	}
+
+	//by default, Bullet will use this near callback
+	static void  defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo);
+
+	virtual	void* allocateCollisionAlgorithm(int size);
+
+	virtual	void freeCollisionAlgorithm(void* ptr);
+
+	btCollisionConfiguration*	getCollisionConfiguration()
+	{
+		return m_collisionConfiguration;
+	}
+
+	const btCollisionConfiguration*	getCollisionConfiguration() const
+	{
+		return m_collisionConfiguration;
+	}
+
+	void	setCollisionConfiguration(btCollisionConfiguration* config)
+	{
+		m_collisionConfiguration = config;
+	}
+
+};
+
+#endif //COLLISION__DISPATCHER_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp	(revision 1963)
@@ -0,0 +1,65 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btCollisionObject.h"
+
+btCollisionObject::btCollisionObject()
+	:	m_broadphaseHandle(0),
+		m_collisionShape(0),
+		m_rootCollisionShape(0),
+		m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
+		m_islandTag1(-1),
+		m_companionId(-1),
+		m_activationState1(1),
+		m_deactivationTime(btScalar(0.)),
+		m_friction(btScalar(0.5)),
+		m_restitution(btScalar(0.)),
+		m_userObjectPointer(0),
+		m_internalType(CO_COLLISION_OBJECT),
+		m_hitFraction(btScalar(1.)),
+		m_ccdSweptSphereRadius(btScalar(0.)),
+		m_ccdMotionThreshold(btScalar(0.)),
+		m_checkCollideWith(false)
+{
+	
+}
+
+btCollisionObject::~btCollisionObject()
+{
+}
+
+void btCollisionObject::setActivationState(int newState) 
+{ 
+	if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+		m_activationState1 = newState;
+}
+
+void btCollisionObject::forceActivationState(int newState)
+{
+	m_activationState1 = newState;
+}
+
+void btCollisionObject::activate(bool forceActivation)
+{
+	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+	{
+		setActivationState(ACTIVE_TAG);
+		m_deactivationTime = btScalar(0.);
+	}
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h	(revision 1963)
@@ -0,0 +1,384 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_OBJECT_H
+#define COLLISION_OBJECT_H
+
+#include "LinearMath/btTransform.h"
+
+//island management, m_activationState1
+#define ACTIVE_TAG 1
+#define ISLAND_SLEEPING 2
+#define WANTS_DEACTIVATION 3
+#define DISABLE_DEACTIVATION 4
+#define DISABLE_SIMULATION 5
+
+struct	btBroadphaseProxy;
+class	btCollisionShape;
+#include "LinearMath/btMotionState.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+
+/// btCollisionObject can be used to manage collision detection objects. 
+/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the btCollisionWorld.
+ATTRIBUTE_ALIGNED16(class)	btCollisionObject
+{
+
+protected:
+
+	btTransform	m_worldTransform;
+
+	///m_interpolationWorldTransform is used for CCD and interpolation
+	///it can be either previous or future (predicted) transform
+	btTransform	m_interpolationWorldTransform;
+	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
+	btVector3	m_interpolationLinearVelocity;
+	btVector3	m_interpolationAngularVelocity;
+	btBroadphaseProxy*		m_broadphaseHandle;
+	btCollisionShape*		m_collisionShape;
+	
+	///m_rootCollisionShape is temporarily used to store the original collision shape
+	///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes
+	///If it is NULL, the m_collisionShape is not temporarily replaced.
+	btCollisionShape*		m_rootCollisionShape;
+
+	int				m_collisionFlags;
+
+	int				m_islandTag1;
+	int				m_companionId;
+
+	int				m_activationState1;
+	btScalar			m_deactivationTime;
+
+	btScalar		m_friction;
+	btScalar		m_restitution;
+
+	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+	void*			m_userObjectPointer;
+
+	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody etc.
+	///do not assign your own m_internalType unless you write a new dynamics object class.
+	int				m_internalType;
+
+	///time of impact calculation
+	btScalar		m_hitFraction; 
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar		m_ccdSweptSphereRadius;
+
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+	btScalar		m_ccdMotionThreshold;
+	
+	/// If some object should have elaborate collision filtering by sub-classes
+	bool			m_checkCollideWith;
+
+	char	m_pad[7];
+
+	virtual bool	checkCollideWithOverride(btCollisionObject* /* co */)
+	{
+		return true;
+	}
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	enum CollisionFlags
+	{
+		CF_STATIC_OBJECT= 1,
+		CF_KINEMATIC_OBJECT= 2,
+		CF_NO_CONTACT_RESPONSE = 4,
+		CF_CUSTOM_MATERIAL_CALLBACK = 8//this allows per-triangle material (friction/restitution)
+	};
+
+	enum	CollisionObjectTypes
+	{
+		CO_COLLISION_OBJECT =1,
+		CO_RIGID_BODY,
+		CO_SOFT_BODY
+	};
+
+	SIMD_FORCE_INLINE bool mergesSimulationIslands() const
+	{
+		///static objects, kinematic and object without contact response don't merge islands
+		return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
+	}
+
+
+	SIMD_FORCE_INLINE bool		isStaticObject() const {
+		return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
+	}
+
+	SIMD_FORCE_INLINE bool		isKinematicObject() const
+	{
+		return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
+	}
+
+	SIMD_FORCE_INLINE bool		isStaticOrKinematicObject() const
+	{
+		return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
+	}
+
+	SIMD_FORCE_INLINE bool		hasContactResponse() const {
+		return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0;
+	}
+
+	
+	btCollisionObject();
+
+	virtual ~btCollisionObject();
+
+	virtual void	setCollisionShape(btCollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+		m_rootCollisionShape = collisionShape;
+	}
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
+	{
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
+	{
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getRootCollisionShape() const
+	{
+		return m_rootCollisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getRootCollisionShape()
+	{
+		return m_rootCollisionShape;
+	}
+
+	///Avoid using this internal API call
+	///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
+	void	internalSetTemporaryCollisionShape(btCollisionShape* collisionShape)
+	{
+		m_collisionShape = collisionShape;
+	}
+
+	int	getActivationState() const { return m_activationState1;}
+	
+	void setActivationState(int newState);
+
+	void	setDeactivationTime(btScalar time)
+	{
+		m_deactivationTime = time;
+	}
+	btScalar	getDeactivationTime() const
+	{
+		return m_deactivationTime;
+	}
+
+	void forceActivationState(int newState);
+
+	void	activate(bool forceActivation = false);
+
+	inline bool isActive() const
+	{
+		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+	}
+
+	void	setRestitution(btScalar rest)
+	{
+		m_restitution = rest;
+	}
+	btScalar	getRestitution() const
+	{
+		return m_restitution;
+	}
+	void	setFriction(btScalar frict)
+	{
+		m_friction = frict;
+	}
+	btScalar	getFriction() const
+	{
+		return m_friction;
+	}
+
+	///reserved for Bullet internal usage
+	int	getInternalType() const
+	{
+		return m_internalType;
+	}
+
+	btTransform&	getWorldTransform()
+	{
+		return m_worldTransform;
+	}
+
+	const btTransform&	getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+
+	void	setWorldTransform(const btTransform& worldTrans)
+	{
+		m_worldTransform = worldTrans;
+	}
+
+
+	SIMD_FORCE_INLINE btBroadphaseProxy*	getBroadphaseHandle()
+	{
+		return m_broadphaseHandle;
+	}
+
+	SIMD_FORCE_INLINE const btBroadphaseProxy*	getBroadphaseHandle() const
+	{
+		return m_broadphaseHandle;
+	}
+
+	void	setBroadphaseHandle(btBroadphaseProxy* handle)
+	{
+		m_broadphaseHandle = handle;
+	}
+
+
+	const btTransform&	getInterpolationWorldTransform() const
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	btTransform&	getInterpolationWorldTransform()
+	{
+		return m_interpolationWorldTransform;
+	}
+
+	void	setInterpolationWorldTransform(const btTransform&	trans)
+	{
+		m_interpolationWorldTransform = trans;
+	}
+
+	void	setInterpolationLinearVelocity(const btVector3& linvel)
+	{
+		m_interpolationLinearVelocity = linvel;
+	}
+
+	void	setInterpolationAngularVelocity(const btVector3& angvel)
+	{
+		m_interpolationAngularVelocity = angvel;
+	}
+
+	const btVector3&	getInterpolationLinearVelocity() const
+	{
+		return m_interpolationLinearVelocity;
+	}
+
+	const btVector3&	getInterpolationAngularVelocity() const
+	{
+		return m_interpolationAngularVelocity;
+	}
+
+	const int getIslandTag() const
+	{
+		return	m_islandTag1;
+	}
+
+	void	setIslandTag(int tag)
+	{
+		m_islandTag1 = tag;
+	}
+
+	const int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
+	const btScalar			getHitFraction() const
+	{
+		return m_hitFraction; 
+	}
+
+	void	setHitFraction(btScalar hitFraction)
+	{
+		m_hitFraction = hitFraction;
+	}
+
+	
+	const int	getCollisionFlags() const
+	{
+		return m_collisionFlags;
+	}
+
+	void	setCollisionFlags(int flags)
+	{
+		m_collisionFlags = flags;
+	}
+	
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	btScalar			getCcdSweptSphereRadius() const
+	{
+		return m_ccdSweptSphereRadius;
+	}
+
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	void	setCcdSweptSphereRadius(btScalar radius)
+	{
+		m_ccdSweptSphereRadius = radius;
+	}
+
+	btScalar 	getCcdMotionThreshold() const
+	{
+		return m_ccdMotionThreshold;
+	}
+
+	btScalar 	getCcdSquareMotionThreshold() const
+	{
+		return m_ccdMotionThreshold*m_ccdMotionThreshold;
+	}
+
+
+
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+	void	setCcdMotionThreshold(btScalar ccdMotionThreshold)
+	{
+		m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+	
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+
+	inline bool checkCollideWith(btCollisionObject* co)
+	{
+		if (m_checkCollideWith)
+			return checkCollideWithOverride(co);
+
+		return true;
+	}
+};
+
+#endif //COLLISION_OBJECT_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp	(revision 1963)
@@ -0,0 +1,725 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCollisionWorld.h"
+#include "btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btStackAlloc.h"
+
+//#define USE_BRUTEFORCE_RAYBROADPHASE 1
+//RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
+//#define RECALCULATE_AABB_RAYCAST 1
+
+//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
+
+
+btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
+:m_dispatcher1(dispatcher),
+m_broadphasePairCache(pairCache),
+m_debugDrawer(0)
+{
+	m_stackAlloc = collisionConfiguration->getStackAllocator();
+	m_dispatchInfo.m_stackAllocator = m_stackAlloc;
+}
+
+
+btCollisionWorld::~btCollisionWorld()
+{
+
+	//clean up remaining objects
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* collisionObject= m_collisionObjects[i];
+
+		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+			getBroadphase()->destroyProxy(bp,m_dispatcher1);
+		}
+	}
+
+
+}
+
+
+
+
+
+
+
+
+
+
+void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+
+	//check that the object isn't already added
+		btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+
+		m_collisionObjects.push_back(collisionObject);
+
+		//calculate new AABB
+		btTransform trans = collisionObject->getWorldTransform();
+
+		btVector3	minAabb;
+		btVector3	maxAabb;
+		collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+
+		int type = collisionObject->getCollisionShape()->getShapeType();
+		collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+			minAabb,
+			maxAabb,
+			type,
+			collisionObject,
+			collisionFilterGroup,
+			collisionFilterMask,
+			m_dispatcher1,0
+			))	;
+
+
+
+
+
+}
+
+void	btCollisionWorld::updateAabbs()
+{
+	BT_PROFILE("updateAabbs");
+
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+
+		//only update aabb of active objects
+		if (colObj->isActive())
+		{
+			btPoint3 minAabb,maxAabb;
+			colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+			//need to increase the aabb for contact thresholds
+			btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+			minAabb -= contactThreshold;
+			maxAabb += contactThreshold;
+
+			btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
+
+			//moving objects should be moderately sized, probably something wrong if not
+			if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
+			{
+				bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+			} else
+			{
+				//something went wrong, investigate
+				//this assert is unwanted in 3D modelers (danger of loosing work)
+				colObj->setActivationState(DISABLE_SIMULATION);
+
+				static bool reportMe = true;
+				if (reportMe && m_debugDrawer)
+				{
+					reportMe = false;
+					m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
+					m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
+					m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+					m_debugDrawer->reportErrorWarning("Thanks.\n");
+				}
+			}
+		}
+	}
+
+}
+
+
+
+void	btCollisionWorld::performDiscreteCollisionDetection()
+{
+	BT_PROFILE("performDiscreteCollisionDetection");
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	updateAabbs();
+
+	{
+		BT_PROFILE("calculateOverlappingPairs");
+		m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+	}
+
+
+	btDispatcher* dispatcher = getDispatcher();
+	{
+		BT_PROFILE("dispatchAllCollisionPairs");
+		if (dispatcher)
+			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+	}
+
+}
+
+
+
+void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+
+
+	//bool removeFromBroadphase = false;
+
+	{
+
+		btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
+			getBroadphase()->destroyProxy(bp,m_dispatcher1);
+			collisionObject->setBroadphaseHandle(0);
+		}
+	}
+
+
+	//swapremove
+	m_collisionObjects.remove(collisionObject);
+
+}
+
+
+
+void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback)
+{
+	btSphereShape pointShape(btScalar(0.0));
+	pointShape.setMargin(0.f);
+	const btConvexShape* castShape = &pointShape;
+
+	if (collisionShape->isConvex())
+	{
+		BT_PROFILE("rayTestConvex");
+		btConvexCast::CastResult castResult;
+		castResult.m_fraction = resultCallback.m_closestHitFraction;
+
+		btConvexShape* convexShape = (btConvexShape*) collisionShape;
+		btVoronoiSimplexSolver	simplexSolver;
+#define USE_SUBSIMPLEX_CONVEX_CAST 1
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+		btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
+#else
+		//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
+		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+
+		if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+		{
+			//add hit
+			if (castResult.m_normal.length2() > btScalar(0.0001))
+			{
+				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+				{
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+					//rotate normal into worldspace
+					castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+
+					castResult.m_normal.normalize();
+					btCollisionWorld::LocalRayResult localRayResult
+						(
+							collisionObject,
+							0,
+							castResult.m_normal,
+							castResult.m_fraction
+						);
+
+					bool normalInWorldSpace = true;
+					resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
+
+				}
+			}
+		}
+	} else {
+		if (collisionShape->isConcave())
+		{
+			BT_PROFILE("rayTestConcave");
+			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				///optimized version for btBvhTriangleMeshShape
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+				//ConvexCast::CastResult
+				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+				{
+					btCollisionWorld::RayResultCallback* m_resultCallback;
+					btCollisionObject*	m_collisionObject;
+					btTriangleMeshShape*	m_triangleMesh;
+
+					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh):
+						btTriangleRaycastCallback(from,to),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh)
+						{
+						}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+
+						btCollisionWorld::LocalRayResult rayResult
+						(m_collisionObject,
+							&shapeInfo,
+							hitNormalLocal,
+							hitFraction);
+
+						bool	normalInWorldSpace = false;
+						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+					}
+
+				};
+
+				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
+			} else
+			{
+				btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape;
+
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+
+				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+				//ConvexCast::CastResult
+
+				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+				{
+					btCollisionWorld::RayResultCallback* m_resultCallback;
+					btCollisionObject*	m_collisionObject;
+					btTriangleMeshShape*	m_triangleMesh;
+
+					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+						btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh):
+						btTriangleRaycastCallback(from,to),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh)
+						{
+						}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+
+						btCollisionWorld::LocalRayResult rayResult
+						(m_collisionObject,
+							&shapeInfo,
+							hitNormalLocal,
+							hitFraction);
+
+						bool	normalInWorldSpace = false;
+						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+
+
+					}
+
+				};
+
+
+				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+				btVector3 rayAabbMinLocal = rayFromLocal;
+				rayAabbMinLocal.setMin(rayToLocal);
+				btVector3 rayAabbMaxLocal = rayFromLocal;
+				rayAabbMaxLocal.setMax(rayToLocal);
+
+				triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+			}
+		} else {
+			BT_PROFILE("rayTestCompound");
+			//todo: use AABB tree or other BVH acceleration structure!
+			if (collisionShape->isCompound())
+			{
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+				int i=0;
+				for (i=0;i<compoundShape->getNumChildShapes();i++)
+				{
+					btTransform childTrans = compoundShape->getChildTransform(i);
+					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+					btTransform childWorldTrans = colObjWorldTransform * childTrans;
+					// replace collision shape so that callback can determine the triangle
+					btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
+					collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+					rayTestSingle(rayFromTrans,rayToTrans,
+						collisionObject,
+						childCollisionShape,
+						childWorldTrans,
+						resultCallback);
+					// restore
+					collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+				}
+			}
+		}
+	}
+}
+
+void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+{
+	if (collisionShape->isConvex())
+	{
+		btConvexCast::CastResult castResult;
+		castResult.m_allowedPenetration = allowedPenetration;
+		castResult.m_fraction = btScalar(1.);//??
+
+		btConvexShape* convexShape = (btConvexShape*) collisionShape;
+		btVoronoiSimplexSolver	simplexSolver;
+		btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+		
+		btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
+		//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
+		//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
+
+		btConvexCast* castPtr = &convexCaster1;
+	
+	
+		
+		if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
+		{
+			//add hit
+			if (castResult.m_normal.length2() > btScalar(0.0001))
+			{
+				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
+				{
+					castResult.m_normal.normalize();
+					btCollisionWorld::LocalConvexResult localConvexResult
+								(
+									collisionObject,
+									0,
+									castResult.m_normal,
+									castResult.m_hitPoint,
+									castResult.m_fraction
+								);
+
+					bool normalInWorldSpace = true;
+					resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
+
+				}
+			}
+		}
+	} else {
+		if (collisionShape->isConcave())
+		{
+			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
+				btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
+				// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
+				btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
+
+				//ConvexCast::CastResult
+				struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
+				{
+					btCollisionWorld::ConvexResultCallback* m_resultCallback;
+					btCollisionObject*	m_collisionObject;
+					btTriangleMeshShape*	m_triangleMesh;
+
+					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh, const btTransform& triangleToWorld):
+						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh)
+						{
+						}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+						if (hitFraction <= m_resultCallback->m_closestHitFraction)
+						{
+
+							btCollisionWorld::LocalConvexResult convexResult
+							(m_collisionObject,
+								&shapeInfo,
+								hitNormalLocal,
+								hitPointLocal,
+								hitFraction);
+
+							bool	normalInWorldSpace = true;
+
+
+							return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+						}
+						return hitFraction;
+					}
+
+				};
+
+				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
+				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+				btVector3 boxMinLocal, boxMaxLocal;
+				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+				triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
+			} else
+			{
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
+				btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
+				// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
+				btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
+
+				//ConvexCast::CastResult
+				struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
+				{
+					btCollisionWorld::ConvexResultCallback* m_resultCallback;
+					btCollisionObject*	m_collisionObject;
+					btTriangleMeshShape*	m_triangleMesh;
+
+					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+						btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*	triangleMesh, const btTransform& triangleToWorld):
+						btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh)
+						{
+						}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+						if (hitFraction <= m_resultCallback->m_closestHitFraction)
+						{
+
+							btCollisionWorld::LocalConvexResult convexResult
+							(m_collisionObject,
+								&shapeInfo,
+								hitNormalLocal,
+								hitPointLocal,
+								hitFraction);
+
+							bool	normalInWorldSpace = false;
+
+							return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
+						}
+						return hitFraction;
+					}
+
+				};
+
+				BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
+				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+				btVector3 boxMinLocal, boxMaxLocal;
+				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+
+				btVector3 rayAabbMinLocal = convexFromLocal;
+				rayAabbMinLocal.setMin(convexToLocal);
+				btVector3 rayAabbMaxLocal = convexFromLocal;
+				rayAabbMaxLocal.setMax(convexToLocal);
+				rayAabbMinLocal += boxMinLocal;
+				rayAabbMaxLocal += boxMaxLocal;
+				triangleMesh->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
+			}
+		} else {
+			//todo: use AABB tree or other BVH acceleration structure!
+			if (collisionShape->isCompound())
+			{
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+				int i=0;
+				for (i=0;i<compoundShape->getNumChildShapes();i++)
+				{
+					btTransform childTrans = compoundShape->getChildTransform(i);
+					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+					btTransform childWorldTrans = colObjWorldTransform * childTrans;
+					// replace collision shape so that callback can determine the triangle
+					btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
+					collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+					objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+						collisionObject,
+						childCollisionShape,
+						childWorldTrans,
+						resultCallback, allowedPenetration);
+					// restore
+					collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+				}
+			}
+		}
+	}
+}
+
+
+struct btSingleRayCallback : public btBroadphaseRayCallback
+{
+
+	btVector3	m_rayFromWorld;
+	btVector3	m_rayToWorld;
+	const btCollisionWorld*	m_world;
+	btCollisionWorld::RayResultCallback&	m_resultCallback;
+
+	btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
+	:m_rayFromWorld(rayFromWorld),
+	m_rayToWorld(rayToWorld),
+	m_world(world),
+	m_resultCallback(resultCallback)
+	{
+
+	}
+
+	virtual bool	process(const btBroadphaseProxy* proxy)
+	{
+		///terminate further ray tests, once the closestHitFraction reached zero
+		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+			return false;
+
+		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+		//only perform raycast if filterMask matches
+		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+			btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+			btVector3 hitNormal;
+			if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			{
+				btTransform	rayFromTrans,rayToTrans;
+				rayFromTrans.setIdentity();
+				rayFromTrans.setOrigin(m_rayFromWorld);
+				rayToTrans.setIdentity();
+				rayToTrans.setOrigin(m_rayToWorld);
+
+				m_world->rayTestSingle(rayFromTrans,rayToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						m_resultCallback);
+			}
+		}
+		return true;
+	}
+};
+
+void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+	BT_PROFILE("rayTest");
+	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
+	btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+#else
+	for (int i=0;i<this->getNumCollisionObjects();i++)
+	{
+		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+	}	
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+
+}
+
+void	btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback) const
+{
+	btTransform	convexFromTrans,convexToTrans;
+	convexFromTrans = convexFromWorld;
+	convexToTrans = convexToWorld;
+	btVector3 castShapeAabbMin, castShapeAabbMax;
+	/* Compute AABB that encompasses angular movement */
+	{
+		btVector3 linVel, angVel;
+		btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
+		btTransform R;
+		R.setIdentity ();
+		R.setRotation (convexFromTrans.getRotation());
+		castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
+	}
+
+	/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
+	// do a ray-shape query using convexCaster (CCD)
+	int i;
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_collisionObjects[i];
+		//only perform raycast if filterMask matches
+		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+			AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
+			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
+			btVector3 hitNormal;
+			if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
+			{
+				objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						resultCallback,
+						getDispatchInfo().m_allowedCcdPenetration);
+			}
+		}
+	}
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h	(revision 1963)
@@ -0,0 +1,404 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+/**
+ * @mainpage Bullet Documentation
+ *
+ * @section intro_sec Introduction
+ * Bullet Collision Detection & Physics SDK
+ *
+ * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
+ *
+ * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions.
+ * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php
+ *
+ * @section install_sec Installation
+ *
+ * @subsection step1 Step 1: Download
+ * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/
+ * @subsection step2 Step 2: Building
+ * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
+ * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
+ * 
+ * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
+ * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam.
+ * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
+ * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/
+ * 
+ * @subsection step3 Step 3: Testing demos
+ * Try to run and experiment with BasicDemo executable as a starting point.
+ * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation.
+ * The Dependencies can be seen in this documentation under Directories
+ * 
+ * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation
+ * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform.
+ * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld.
+ * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras)
+ * Bullet Collision Detection can also be used without the Dynamics/Extras.
+ * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo.
+ * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation.
+ * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector.
+ *
+ * @section copyright Copyright
+ * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon
+ * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky,
+ * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt.
+ * 
+ */
+ 
+ 
+
+#ifndef COLLISION_WORLD_H
+#define COLLISION_WORLD_H
+
+class btStackAlloc;
+class btCollisionShape;
+class btConvexShape;
+class btBroadphaseInterface;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btCollisionObject.h"
+#include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///CollisionWorld is interface and container for the collision detection
+class btCollisionWorld
+{
+
+	
+protected:
+
+	btAlignedObjectArray<btCollisionObject*>	m_collisionObjects;
+	
+	btDispatcher*	m_dispatcher1;
+
+	btDispatcherInfo	m_dispatchInfo;
+
+	btStackAlloc*	m_stackAlloc;
+
+	btBroadphaseInterface*	m_broadphasePairCache;
+
+	btIDebugDraw*	m_debugDrawer;
+
+	
+public:
+
+	//this constructor doesn't own the dispatcher and paircache/broadphase
+	btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btCollisionWorld();
+
+	void	setBroadphase(btBroadphaseInterface*	pairCache)
+	{
+		m_broadphasePairCache = pairCache;
+	}
+
+	const btBroadphaseInterface*	getBroadphase() const
+	{
+		return m_broadphasePairCache;
+	}
+
+	btBroadphaseInterface*	getBroadphase()
+	{
+		return m_broadphasePairCache;
+	}
+
+	btOverlappingPairCache*	getPairCache()
+	{
+		return m_broadphasePairCache->getOverlappingPairCache();
+	}
+
+
+	btDispatcher*	getDispatcher()
+	{
+		return m_dispatcher1;
+	}
+
+	const btDispatcher*	getDispatcher() const
+	{
+		return m_dispatcher1;
+	}
+
+	virtual void	updateAabbs();
+
+	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
+	{
+			m_debugDrawer = debugDrawer;
+	}
+
+	virtual btIDebugDraw*	getDebugDrawer()
+	{
+		return m_debugDrawer;
+	}
+
+
+	///LocalShapeInfo gives extra information for complex shapes
+	///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart
+	struct	LocalShapeInfo
+	{
+		int	m_shapePart;
+		int	m_triangleIndex;
+		
+		//const btCollisionShape*	m_shapeTemp;
+		//const btTransform*	m_shapeLocalTransform;
+	};
+
+	struct	LocalRayResult
+	{
+		LocalRayResult(btCollisionObject*	collisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			btScalar hitFraction)
+		:m_collisionObject(collisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		btCollisionObject*		m_collisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		btVector3				m_hitNormalLocal;
+		btScalar				m_hitFraction;
+
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	RayResultCallback
+	{
+		btScalar	m_closestHitFraction;
+		btCollisionObject*		m_collisionObject;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+
+		virtual ~RayResultCallback()
+		{
+		}
+		bool	hasHit() const
+		{
+			return (m_collisionObject != 0);
+		}
+
+		RayResultCallback()
+			:m_closestHitFraction(btScalar(1.)),
+			m_collisionObject(0),
+			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+		{
+		}
+
+		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+		{
+			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+		}
+
+
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0;
+	};
+
+	struct	ClosestRayResultCallback : public RayResultCallback
+	{
+		ClosestRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld)
+		{
+		}
+
+		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_rayToWorld;
+
+		btVector3	m_hitNormalWorld;
+		btVector3	m_hitPointWorld;
+			
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		{
+			//caller already does the filter on the m_closestHitFraction
+			btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
+			
+			m_closestHitFraction = rayResult.m_hitFraction;
+			m_collisionObject = rayResult.m_collisionObject;
+			if (normalInWorldSpace)
+			{
+				m_hitNormalWorld = rayResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+			}
+			m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			return rayResult.m_hitFraction;
+		}
+	};
+
+
+	struct LocalConvexResult
+	{
+		LocalConvexResult(btCollisionObject*	hitCollisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			const btVector3&		hitPointLocal,
+			btScalar hitFraction
+			)
+		:m_hitCollisionObject(hitCollisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitPointLocal(hitPointLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		btCollisionObject*		m_hitCollisionObject;
+		LocalShapeInfo*			m_localShapeInfo;
+		btVector3				m_hitNormalLocal;
+		btVector3				m_hitPointLocal;
+		btScalar				m_hitFraction;
+	};
+
+	///RayResultCallback is used to report new raycast results
+	struct	ConvexResultCallback
+	{
+		btScalar	m_closestHitFraction;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+		
+		ConvexResultCallback()
+			:m_closestHitFraction(btScalar(1.)),
+			m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+		{
+		}
+
+		virtual ~ConvexResultCallback()
+		{
+		}
+		
+		bool	hasHit() const
+		{
+			return (m_closestHitFraction < btScalar(1.));
+		}
+
+		
+
+		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+		{
+			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			return collides;
+		}
+
+		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0;
+	};
+
+	struct	ClosestConvexResultCallback : public ConvexResultCallback
+	{
+		ClosestConvexResultCallback(const btVector3&	convexFromWorld,const btVector3&	convexToWorld)
+		:m_convexFromWorld(convexFromWorld),
+		m_convexToWorld(convexToWorld),
+		m_hitCollisionObject(0)
+		{
+		}
+
+		btVector3	m_convexFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_convexToWorld;
+
+		btVector3	m_hitNormalWorld;
+		btVector3	m_hitPointWorld;
+		btCollisionObject*	m_hitCollisionObject;
+		
+		virtual	btScalar	addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
+		{
+//caller already does the filter on the m_closestHitFraction
+			btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
+						
+			m_closestHitFraction = convexResult.m_hitFraction;
+			m_hitCollisionObject = convexResult.m_hitCollisionObject;
+			if (normalInWorldSpace)
+			{
+				m_hitNormalWorld = convexResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+			}
+			m_hitPointWorld = convexResult.m_hitPointLocal;
+			return convexResult.m_hitFraction;
+		}
+	};
+
+	int	getNumCollisionObjects() const
+	{
+		return int(m_collisionObjects.size());
+	}
+
+	/// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
+	/// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+	void	rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+	// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
+	// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
+	void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback) const;
+
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback);
+
+	/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
+	static void	objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  ConvexResultCallback& resultCallback, btScalar	allowedPenetration);
+
+	void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+	btCollisionObjectArray& getCollisionObjectArray()
+	{
+		return m_collisionObjects;
+	}
+
+	const btCollisionObjectArray& getCollisionObjectArray() const
+	{
+		return m_collisionObjects;
+	}
+
+
+	void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void	performDiscreteCollisionDetection();
+
+	btDispatcherInfo& getDispatchInfo()
+	{
+		return m_dispatchInfo;
+	}
+
+	const btDispatcherInfo& getDispatchInfo() const
+	{
+		return m_dispatchInfo;
+	}
+
+};
+
+
+#endif //COLLISION_WORLD_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,287 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+:btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_sharedManifold(ci.m_manifold)
+{
+	m_ownsManifold = false;
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+	assert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+	int numChildren = compoundShape->getNumChildShapes();
+	int i;
+	
+	m_childCollisionAlgorithms.resize(numChildren);
+	for (i=0;i<numChildren;i++)
+	{
+		if (compoundShape->getDynamicAabbTree())
+		{
+			m_childCollisionAlgorithms[i] = 0;
+		} else
+		{
+			btCollisionShape* tmpShape = colObj->getCollisionShape();
+			btCollisionShape* childShape = compoundShape->getChildShape(i);
+			colObj->internalSetTemporaryCollisionShape( childShape );
+			m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
+			colObj->internalSetTemporaryCollisionShape( tmpShape );
+		}
+	}
+}
+
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		if (m_childCollisionAlgorithms[i])
+		{
+			m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+		}
+	}
+}
+
+
+
+
+struct	btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+	btCollisionObject* m_compoundColObj;
+	btCollisionObject* m_otherObj;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	btCollisionAlgorithm**	m_childCollisionAlgorithms;
+	btPersistentManifold*	m_sharedManifold;
+
+
+
+
+	btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
+		:m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithms(childCollisionAlgorithms),
+		m_sharedManifold(sharedManifold)
+	{
+
+	}
+
+
+	void	ProcessChildShape(btCollisionShape* childShape,int index)
+	{
+		
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+
+
+		//backup
+		btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+		btTransform	orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
+		const btTransform& childTrans = compoundShape->getChildTransform(index);
+		btTransform	newChildWorldTrans = orgTrans*childTrans ;
+
+		//perform an AABB check first
+		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+		m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+
+			m_compoundColObj->setWorldTransform( newChildWorldTrans);
+			m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
+
+			//the contactpoint is still projected back using the original inverted worldtrans
+			btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
+			m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
+
+			if (!m_childCollisionAlgorithms[index])
+				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+			m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
+			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btVector3 worldAabbMin,worldAabbMax;
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
+				m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
+			}
+			
+			//revert back transform
+			m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
+			m_compoundColObj->setWorldTransform(  orgTrans );
+			m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
+		}
+	}
+	void		Process(const btDbvtNode* leaf)
+	{
+		int index = leaf->dataAsInt;
+
+		btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+		btCollisionShape* childShape = compoundShape->getChildShape(index);
+		if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+		{
+			btVector3 worldAabbMin,worldAabbMax;
+			btTransform	orgTrans = m_compoundColObj->getWorldTransform();
+			btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+			m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+		}
+		ProcessChildShape(childShape,index);
+
+	}
+};
+
+
+
+
+
+
+void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	assert (colObj->getCollisionShape()->isCompound());
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	btDbvt* tree = compoundShape->getDynamicAabbTree();
+	//use a dynamic aabb tree to cull potential child-overlaps
+	btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
+
+
+	if (tree)
+	{
+
+		btVector3 localAabbMin,localAabbMax;
+		btTransform otherInCompoundSpace;
+		otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
+		otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+
+		const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		//process all children, that overlap with  the given AABB bounds
+		tree->collideTV(tree->m_root,bounds,callback);
+
+	} else
+	{
+		//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		for (i=0;i<numChildren;i++)
+		{
+			callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+		}
+	}
+
+	{
+				//iterate over all children, perform an AABB check inside ProcessChildShape
+		int numChildren = m_childCollisionAlgorithms.size();
+		int i;
+		btManifoldArray	manifoldArray;
+
+		for (i=0;i<numChildren;i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				btCollisionShape* childShape = compoundShape->getChildShape(i);
+			//if not longer overlapping, remove the algorithm
+				btTransform	orgTrans = colObj->getWorldTransform();
+				btTransform	orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+				const btTransform& childTrans = compoundShape->getChildTransform(i);
+				btTransform	newChildWorldTrans = orgTrans*childTrans ;
+
+				//perform an AABB check first
+				btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+				otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				{
+					m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+					m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+					m_childCollisionAlgorithms[i] = 0;
+				}
+
+			}
+			
+		}
+
+		
+
+	}
+}
+
+btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	assert (colObj->getCollisionShape()->isCompound());
+	
+	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
+	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+	//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+	//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+	//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+	//then use each overlapping node AABB against Tree0
+	//and vise versa.
+
+	btScalar hitFraction = btScalar(1.);
+
+	int numChildren = m_childCollisionAlgorithms.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		//temporarily exchange parent btCollisionShape with childShape, and recurse
+		btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+		//backup
+		btTransform	orgTrans = colObj->getWorldTransform();
+	
+		const btTransform& childTrans = compoundShape->getChildTransform(i);
+		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		colObj->setWorldTransform( orgTrans*childTrans );
+
+		btCollisionShape* tmpShape = colObj->getCollisionShape();
+		colObj->internalSetTemporaryCollisionShape( childShape );
+		btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+		if (frac<hitFraction)
+		{
+			hitFraction = frac;
+		}
+		//revert back
+		colObj->internalSetTemporaryCollisionShape( tmpShape);
+		colObj->setWorldTransform( orgTrans);
+	}
+	return hitFraction;
+
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,79 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COMPOUND_COLLISION_ALGORITHM_H
+#define COMPOUND_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+class btDispatcher;
+
+/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
+class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
+{
+	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+	bool m_isSwapped;
+
+	class btPersistentManifold*	m_sharedManifold;
+	bool					m_ownsManifold;
+	
+public:
+
+	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btCompoundCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		int i;
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+		}
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //COMPOUND_COLLISION_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,317 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
+{
+}
+
+btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
+{
+}
+
+void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	if (m_btConvexTriangleCallback.m_manifoldPtr)
+	{
+		manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
+	}
+}
+
+
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+	  m_dispatcher(dispatcher),
+	m_dispatchInfoPtr(0)
+{
+	m_convexBody = isSwapped? body1:body0;
+	m_triBody = isSwapped? body0:body1;
+	
+	  //
+	  // create the manifold from the dispatcher 'manifold pool'
+	  //
+	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
+
+  	  clearCache();
+}
+
+btConvexTriangleCallback::~btConvexTriangleCallback()
+{
+	clearCache();
+	m_dispatcher->releaseManifold( m_manifoldPtr );
+  
+}
+  
+
+void	btConvexTriangleCallback::clearCache()
+{
+	m_dispatcher->clearManifold(m_manifoldPtr);
+};
+
+
+
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+ 
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+
+
+	//aabb filter is already applied!	
+
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher1 = m_dispatcher;
+
+	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+
+
+	
+	///debug drawing of the overlapping triangles
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
+	{
+		btVector3 color(255,255,0);
+		btTransform& tr = ob->getWorldTransform();
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+
+		//btVector3 center = triangle[0] + triangle[1]+triangle[2];
+		//center *= btScalar(0.333333);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
+		//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
+
+	}
+
+
+	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+	
+	if (m_convexBody->getCollisionShape()->isConvex())
+	{
+		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		tm.setMargin(m_collisionMarginTriangle);
+		
+		btCollisionShape* tmpShape = ob->getCollisionShape();
+		ob->internalSetTemporaryCollisionShape( &tm );
+		
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
+		///this should use the btDispatcher, so the actual registered algorithm is used
+		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+
+		m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
+	//	cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+//		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+		ob->internalSetTemporaryCollisionShape( tmpShape);
+	}
+
+
+}
+
+
+
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle;
+	m_resultOut = resultOut;
+
+	//recalc aabbs
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
+	btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
+	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
+	btScalar extraMargin = collisionMarginTriangle;
+	btVector3 extra(extraMargin,extraMargin,extraMargin);
+
+	m_aabbMax += extra;
+	m_aabbMin -= extra;
+	
+}
+
+void btConvexConcaveCollisionAlgorithm::clearCache()
+{
+	m_btConvexTriangleCallback.clearCache();
+
+}
+
+void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	
+	
+	btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+	if (triBody->getCollisionShape()->isConcave())
+	{
+
+
+		btCollisionObject*	triOb = triBody;
+		btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+		
+		if (convexBody->getCollisionShape()->isConvex())
+		{
+			btScalar collisionMarginTriangle = concaveShape->getMargin();
+					
+			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
+
+			//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+			//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+
+			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+			
+			resultOut->refreshContactPoints();
+	
+		}
+	
+	}
+
+}
+
+
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+	//only perform CCD above a certain threshold, this prevents blocking on the long run
+	//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+	btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+	if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+	{
+		return btScalar(1.);
+	}
+
+	//const btVector3& from = convexbody->m_worldTransform.getOrigin();
+	//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+	//todo: only do if the motion exceeds the 'radius'
+
+	btTransform triInv = triBody->getWorldTransform().inverse();
+	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	{
+		btTransform m_ccdSphereFromTrans;
+		btTransform m_ccdSphereToTrans;
+		btTransform	m_meshTransform;
+
+		btScalar	m_ccdSphereRadius;
+		btScalar	m_hitFraction;
+	
+
+		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+			:m_ccdSphereFromTrans(from),
+			m_ccdSphereToTrans(to),
+			m_ccdSphereRadius(ccdSphereRadius),
+			m_hitFraction(hitFraction)
+		{			
+		}
+		
+		
+		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)partId;
+			(void)triangleIndex;
+			//do a swept sphere for now
+			btTransform ident;
+			ident.setIdentity();
+			btConvexCast::CastResult castResult;
+			castResult.m_fraction = m_hitFraction;
+			btSphereShape	pointShape(m_ccdSphereRadius);
+			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
+			btVoronoiSimplexSolver	simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+			//local space?
+
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+				ident,ident,castResult))
+			{
+				if (m_hitFraction > castResult.m_fraction)
+					m_hitFraction = castResult.m_fraction;
+			}
+
+		}
+
+	};
+
+
+	
+
+	
+	if (triBody->getCollisionShape()->isConcave())
+	{
+		btVector3 rayAabbMin = convexFromLocal.getOrigin();
+		rayAabbMin.setMin(convexToLocal.getOrigin());
+		btVector3 rayAabbMax = convexFromLocal.getOrigin();
+		rayAabbMax.setMax(convexToLocal.getOrigin());
+		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+		btScalar curHitFraction = btScalar(1.); //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+		raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+		btCollisionObject* concavebody = triBody;
+
+		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+		
+		if (triangleMesh)
+		{
+			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+		}
+	
+
+
+		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+		{
+			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			return raycastCallback.m_hitFraction;
+		}
+	}
+
+	return btScalar(1.);
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btCollisionCreateFunc.h"
+
+///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
+class btConvexTriangleCallback : public btTriangleCallback
+{
+	btCollisionObject* m_convexBody;
+	btCollisionObject* m_triBody;
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax ;
+
+	btManifoldResult* m_resultOut;
+
+	btDispatcher*	m_dispatcher;
+	const btDispatcherInfo* m_dispatchInfoPtr;
+	btScalar m_collisionMarginTriangle;
+	
+public:
+int	m_triangleCount;
+	
+	btPersistentManifold*	m_manifoldPtr;
+
+	btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual ~btConvexTriangleCallback();
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+	
+	void clearCache();
+
+	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
+	{
+		return m_aabbMin;
+	}
+	SIMD_FORCE_INLINE const btVector3& getAabbMax() const
+	{
+		return m_aabbMax;
+	}
+
+};
+
+
+
+
+/// btConvexConcaveCollisionAlgorithm  supports collision between convex shapes and (concave) trianges meshes.
+class btConvexConcaveCollisionAlgorithm  : public btCollisionAlgorithm
+{
+
+	bool	m_isSwapped;
+
+	btConvexTriangleCallback m_btConvexTriangleCallback;
+
+
+public:
+
+	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btConvexConcaveCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
+	
+	void	clearCache();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,248 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexConvexAlgorithm.h"
+
+//#include <stdio.h>
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+
+
+
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+
+
+
+
+
+
+
+
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+}
+
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+: btCollisionAlgorithm(ci),
+m_gjkPairDetector(0,0,simplexSolver,pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false)
+{
+	(void)body0;
+	(void)body1;
+
+
+}
+
+
+
+
+btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+#ifdef USE_BT_GJKEPA
+	btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
+	btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
+	const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
+	btGjkEpaSolver::sResults	results;
+	if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
+								shape1,body1->getWorldTransform(),
+								radialmargin,results))
+		{
+		dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		}
+#else
+
+	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+	
+	btGjkPairDetector::ClosestPointInput input;
+
+	//TODO: if (dispatchInfo.m_useContinuous)
+	m_gjkPairDetector.setMinkowskiA(min0);
+	m_gjkPairDetector.setMinkowskiB(min1);
+	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+	input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+
+//	input.m_maximumDistanceSquared = btScalar(1e30);
+	
+	input.m_transformA = body0->getWorldTransform();
+	input.m_transformB = body1->getWorldTransform();
+	
+	m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+#endif
+
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+
+
+bool disableCcd = false;
+btScalar	btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+    
+	///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+	///col0->m_worldTransform,
+	btScalar resultFraction = btScalar(1.);
+
+
+	btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+	btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
+    
+	if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+		squareMot1 < col1->getCcdSquareMotionThreshold())
+		return resultFraction;
+
+	if (disableCcd)
+		return btScalar(1.);
+
+
+	//An adhoc way of testing the Continuous Collision Detection algorithms
+	//One object is approximated as a sphere, to simplify things
+	//Starting in penetration should report no time of impact
+	//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+	//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+		
+	/// Convex0 against sphere for Convex1
+	{
+		btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
+
+		btSphereShape	sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()> result.m_fraction)
+				col0->setHitFraction( result.m_fraction );
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+		
+		
+
+
+	}
+
+	/// Sphere (for convex0) against Convex1
+	{
+		btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
+
+		btSphereShape	sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
+		btConvexCast::CastResult result;
+		btVoronoiSimplexSolver voronoiSimplex;
+		//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+		///Simplification, one object is simplified as a sphere
+		btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
+		//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+		if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+			col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
+		{
+		
+			//store result.m_fraction in both bodies
+		
+			if (col0->getHitFraction()	> result.m_fraction)
+				col0->setHitFraction( result.m_fraction);
+
+			if (col1->getHitFraction() > result.m_fraction)
+				col1->setHitFraction( result.m_fraction);
+
+			if (resultFraction > result.m_fraction)
+				resultFraction = result.m_fraction;
+
+		}
+	}
+	
+	return resultFraction;
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h	(revision 1963)
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_CONVEX_ALGORITHM_H
+#define CONVEX_CONVEX_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+class btConvexPenetrationDepthSolver;
+
+///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+class btConvexConvexAlgorithm : public btCollisionAlgorithm
+{
+	btGjkPairDetector m_gjkPairDetector;
+public:
+
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+
+public:
+
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+
+	virtual ~btConvexConvexAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		///should we use m_ownManifold to avoid adding duplicates?
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
+	void	setLowLevelOfDetail(bool useLowLevel);
+
+
+	const btPersistentManifold*	getManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		btConvexPenetrationDepthSolver*		m_pdSolver;
+		btSimplexSolverInterface*			m_simplexSolver;
+		
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+		}
+	};
+
+
+};
+
+#endif //CONVEX_CONVEX_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,108 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexPlaneCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+//#include <stdio.h>
+
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+	btCollisionObject* convexObj = m_isSwapped? col1 : col0;
+	btCollisionObject* planeObj = m_isSwapped? col0 : col1;
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj);
+		m_ownManifold = true;
+	}
+}
+
+
+btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+
+
+void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+	btCollisionObject* planeObj = m_isSwapped? body0: body1;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
+
+	bool hasCollision = false;
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	const btScalar& planeConstant = planeShape->getPlaneConstant();
+	btTransform planeInConvex;
+	planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
+	btTransform convexInPlaneTrans;
+	convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
+
+	btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
+	btVector3 vtxInPlane = convexInPlaneTrans(vtx);
+	btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
+
+	btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
+	btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;
+
+	hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	if (hasCollision)
+	{
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+		btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;
+		btVector3 pOnB = vtxInPlaneWorld;
+		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+	}
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
+}
+
+btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H
+#define CONVEX_PLANE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_isSwapped;
+	
+public:
+
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btConvexPlaneCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp	(revision 1963)
@@ -0,0 +1,291 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btDefaultCollisionConfiguration.h"
+
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+
+
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btPoolAllocator.h"
+
+
+
+
+
+btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
+//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
+{
+
+	void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
+	m_simplexSolver = new (mem)btVoronoiSimplexSolver();
+	
+#define USE_EPA 1
+#ifdef USE_EPA
+	mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+	m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+#else
+	mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+	m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+#endif//USE_EPA	
+	
+
+	//default CreationFunctions, filling the m_doubleDispatch table
+	mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
+	m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+	m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16);
+	m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
+	m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
+	m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
+	mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
+	m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc;
+	
+	mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
+	m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+	m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
+	m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
+	m_boxSphereCF->m_swapped = true;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+	m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
+	m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+	m_triangleSphereCF->m_swapped = true;
+	
+	mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16);
+	m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc;
+
+	//convex versus plane
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16);
+	m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc;
+	m_planeConvexCF->m_swapped = true;
+	
+	///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+	int maxSize = sizeof(btConvexConvexAlgorithm);
+	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
+	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
+	int maxSize4 = sizeof(btEmptyAlgorithm);
+	
+	int	collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
+
+	if (constructionInfo.m_stackAlloc)
+	{
+		m_ownsStackAllocator = false;
+		this->m_stackAlloc = constructionInfo.m_stackAlloc;
+	} else
+	{
+		m_ownsStackAllocator = true;
+		void* mem = btAlignedAlloc(sizeof(btStackAlloc),16);
+		m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize);
+	}
+		
+	if (constructionInfo.m_persistentManifoldPool)
+	{
+		m_ownsPersistentManifoldPool = false;
+		m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool;
+	} else
+	{
+		m_ownsPersistentManifoldPool = true;
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+		m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
+	}
+	
+	if (constructionInfo.m_collisionAlgorithmPool)
+	{
+		m_ownsCollisionAlgorithmPool = false;
+		m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool;
+	} else
+	{
+		m_ownsCollisionAlgorithmPool = true;
+		void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+		m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+	}
+
+
+}
+
+btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
+{
+	if (m_ownsStackAllocator)
+	{
+		m_stackAlloc->destroy();
+		m_stackAlloc->~btStackAlloc();
+		btAlignedFree(m_stackAlloc);
+	}
+	if (m_ownsCollisionAlgorithmPool)
+	{
+		m_collisionAlgorithmPool->~btPoolAllocator();
+		btAlignedFree(m_collisionAlgorithmPool);
+	}
+	if (m_ownsPersistentManifoldPool)
+	{
+		m_persistentManifoldPool->~btPoolAllocator();
+		btAlignedFree(m_persistentManifoldPool);
+	}
+
+	m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_convexConvexCreateFunc);
+
+	m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_convexConcaveCreateFunc);
+	m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_swappedConvexConcaveCreateFunc);
+
+	m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_compoundCreateFunc);
+
+	m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_swappedCompoundCreateFunc);
+
+	m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_emptyCreateFunc);
+
+	m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereSphereCF);
+
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereBoxCF);
+	m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_boxSphereCF);
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_sphereTriangleCF);
+	m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_triangleSphereCF);
+	m_boxBoxCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_boxBoxCF);
+
+	m_convexPlaneCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_convexPlaneCF);
+	m_planeConvexCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree( m_planeConvexCF);
+
+	m_simplexSolver->~btVoronoiSimplexSolver();
+	btAlignedFree(m_simplexSolver);
+
+	m_pdSolver->~btConvexPenetrationDepthSolver();
+	
+	btAlignedFree(m_pdSolver);
+
+
+}
+
+
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereBoxCF;
+	}
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_boxSphereCF;
+	}
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereTriangleCF;
+	}
+
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE  ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_triangleSphereCF;
+	} 
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return m_boxBoxCF;
+	}
+	
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
+	
+
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
+		return m_swappedConvexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	} else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h	(revision 1963)
@@ -0,0 +1,118 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DEFAULT_COLLISION_CONFIGURATION
+#define BT_DEFAULT_COLLISION_CONFIGURATION
+
+#include "btCollisionConfiguration.h"
+class btVoronoiSimplexSolver;
+class btConvexPenetrationDepthSolver;
+
+struct	btDefaultCollisionConstructionInfo
+{
+	btStackAlloc*		m_stackAlloc;
+	btPoolAllocator*	m_persistentManifoldPool;
+	btPoolAllocator*	m_collisionAlgorithmPool;
+	int					m_defaultMaxPersistentManifoldPoolSize;
+	int					m_defaultMaxCollisionAlgorithmPoolSize;
+	int					m_defaultStackAllocatorSize;
+
+	btDefaultCollisionConstructionInfo()
+		:m_stackAlloc(0),
+		m_persistentManifoldPool(0),
+		m_collisionAlgorithmPool(0),
+		m_defaultMaxPersistentManifoldPoolSize(65535),
+		m_defaultMaxCollisionAlgorithmPoolSize(65535),
+		m_defaultStackAllocatorSize(5*1024*1024)
+	{
+	}
+};
+
+
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator, pool memory allocators
+///todo: describe the meaning
+class	btDefaultCollisionConfiguration : public btCollisionConfiguration
+{
+
+protected:
+
+	int	m_persistentManifoldPoolSize;
+	
+	btStackAlloc*	m_stackAlloc;
+	bool	m_ownsStackAllocator;
+
+	btPoolAllocator*	m_persistentManifoldPool;
+	bool	m_ownsPersistentManifoldPool;
+
+
+	btPoolAllocator*	m_collisionAlgorithmPool;
+	bool	m_ownsCollisionAlgorithmPool;
+
+	//default simplex/penetration depth solvers
+	btVoronoiSimplexSolver*	m_simplexSolver;
+	btConvexPenetrationDepthSolver*	m_pdSolver;
+	
+	//default CreationFunctions, filling the m_doubleDispatch table
+	btCollisionAlgorithmCreateFunc*	m_convexConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_convexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedConvexConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_compoundCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
+	btCollisionAlgorithmCreateFunc* m_boxSphereCF;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+	btCollisionAlgorithmCreateFunc* m_boxBoxCF;
+	btCollisionAlgorithmCreateFunc*	m_sphereTriangleCF;
+	btCollisionAlgorithmCreateFunc*	m_triangleSphereCF;
+	btCollisionAlgorithmCreateFunc*	m_planeConvexCF;
+	btCollisionAlgorithmCreateFunc*	m_convexPlaneCF;
+	
+public:
+
+
+	btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
+
+	virtual ~btDefaultCollisionConfiguration();
+
+		///memory pools
+	virtual btPoolAllocator* getPersistentManifoldPool()
+	{
+		return m_persistentManifoldPool;
+	}
+
+	virtual btPoolAllocator* getCollisionAlgorithmPool()
+	{
+		return m_collisionAlgorithmPool;
+	}
+
+	virtual btStackAlloc*	getStackAllocator()
+	{
+		return m_stackAlloc;
+	}
+
+
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+
+};
+
+#endif //BT_DEFAULT_COLLISION_CONFIGURATION
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,34 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btEmptyCollisionAlgorithm.h"
+
+
+
+btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+	: btCollisionAlgorithm(ci)
+{
+}
+
+void btEmptyAlgorithm::processCollision (btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+}
+
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+	return btScalar(1.);
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,54 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef EMPTY_ALGORITH
+#define EMPTY_ALGORITH
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+#define ATTRIBUTE_ALIGNED(a)
+
+///EmptyAlgorithm is a stub for unsupported collision pairs.
+///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
+class btEmptyAlgorithm : public btCollisionAlgorithm
+{
+
+public:
+	
+	btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci);
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+	}
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			(void)body0;
+			(void)body1;
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
+			return new(mem) btEmptyAlgorithm(ci);
+		}
+	};
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //EMPTY_ALGORITH
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp	(revision 1963)
@@ -0,0 +1,119 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback		gContactAddedCallback=0;
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btScalar friction = body0->getFriction() * body1->getFriction();
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+inline btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	return body0->getRestitution() * body1->getRestitution();
+}
+
+
+
+btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+		:m_manifoldPtr(0),
+		m_body0(body0),
+		m_body1(body1)
+{
+	m_rootTransA = body0->getWorldTransform();
+	m_rootTransB = body1->getWorldTransform();
+}
+
+
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+{
+	assert(m_manifoldPtr);
+	//order in manifold needs to match
+	
+	if (depth > m_manifoldPtr->getContactBreakingThreshold())
+		return;
+
+	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+
+	btVector3 localA;
+	btVector3 localB;
+	
+	if (isSwapped)
+	{
+		localA = m_rootTransB.invXform(pointA );
+		localB = m_rootTransA.invXform(pointInWorld);
+	} else
+	{
+		localA = m_rootTransA.invXform(pointA );
+		localB = m_rootTransB.invXform(pointInWorld);
+	}
+
+	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+	newPt.m_positionWorldOnA = pointA;
+	newPt.m_positionWorldOnB = pointInWorld;
+	
+	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
+
+	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
+	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+
+   //BP mod, store contact triangles.
+   newPt.m_partId0 = m_partId0;
+   newPt.m_partId1 = m_partId1;
+   newPt.m_index0  = m_index0;
+   newPt.m_index1  = m_index1;
+	
+	///todo, check this for any side effects
+	if (insertIndex >= 0)
+	{
+		//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+		m_manifoldPtr->replaceContactPoint(newPt,insertIndex);
+	} else
+	{
+		insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
+	}
+	
+	//User can override friction and/or restitution
+	if (gContactAddedCallback &&
+		//and if either of the two bodies requires custom material
+		 ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+		   (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+	{
+		//experimental feature info, for per-triangle material etc.
+		btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+		btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
+	}
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h	(revision 1963)
@@ -0,0 +1,102 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef MANIFOLD_RESULT_H
+#define MANIFOLD_RESULT_H
+
+class btCollisionObject;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+#include "LinearMath/btTransform.h"
+
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
+extern ContactAddedCallback		gContactAddedCallback;
+
+
+
+///btManifoldResult is a helper class to manage  contact results.
+class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+	btPersistentManifold* m_manifoldPtr;
+
+	//we need this for compounds
+	btTransform	m_rootTransA;
+	btTransform	m_rootTransB;
+
+	btCollisionObject* m_body0;
+	btCollisionObject* m_body1;
+	int	m_partId0;
+	int m_partId1;
+	int m_index0;
+	int m_index1;
+public:
+
+	btManifoldResult()
+	{
+	}
+
+	btManifoldResult(btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btManifoldResult() {};
+
+	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	{
+		m_manifoldPtr = manifoldPtr;
+	}
+
+	const btPersistentManifold*	getPersistentManifold() const
+	{
+		return m_manifoldPtr;
+	}
+	btPersistentManifold*	getPersistentManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+	{
+			m_partId0=partId0;
+			m_partId1=partId1;
+			m_index0=index0;
+			m_index1=index1;		
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
+
+	SIMD_FORCE_INLINE	void refreshContactPoints()
+	{
+		btAssert(m_manifoldPtr);
+		if (!m_manifoldPtr->getNumContacts())
+			return;
+
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+
+		if (isSwapped)
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransB,m_rootTransA);
+		} else
+		{
+			m_manifoldPtr->refreshContactPoints(m_rootTransA,m_rootTransB);
+		}
+	}
+
+
+};
+
+#endif //MANIFOLD_RESULT_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp	(revision 1963)
@@ -0,0 +1,384 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btScalar.h"
+#include "btSimulationIslandManager.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+btSimulationIslandManager::btSimulationIslandManager()
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+		m_unionFind.reset(n);
+}
+		
+
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+{
+	
+	{
+		btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
+
+		for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
+		{
+			const btBroadphasePair& collisionPair = pairPtr[i];
+			btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+			btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+			if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+				((colObj1) && ((colObj1)->mergesSimulationIslands())))
+			{
+
+				m_unionFind.unite((colObj0)->getIslandTag(),
+					(colObj1)->getIslandTag());
+			}
+		}
+	}
+}
+
+
+void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+	
+	initUnionFind( int (colWorld->getCollisionObjectArray().size()));
+	
+	// put the index into m_controllers into m_tag	
+	{
+		
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+		{
+			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
+			collisionObject->setIslandTag(index);
+			collisionObject->setCompanionId(-1);
+			collisionObject->setHitFraction(btScalar(1.));
+			index++;
+			
+		}
+	}
+	// do the union find
+	
+	findUnions(dispatcher,colWorld);
+	
+
+	
+}
+
+
+
+
+void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+	// put the islandId ('find' value) into m_tag	
+	{
+		
+		
+		int index = 0;
+		int i;
+		for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+		{
+			btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+			if (!collisionObject->isStaticOrKinematicObject())
+			{
+				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setCompanionId(-1);
+			} else
+			{
+				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
+			}
+			index++;
+		}
+	}
+}
+
+inline	int	getIslandId(const btPersistentManifold* lhs)
+{
+	int islandId;
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+	islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+	return islandId;
+
+}
+
+
+
+/// function object that routes calls to operator<
+class btPersistentManifoldSortPredicate
+{
+	public:
+
+		SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
+		{
+			return getIslandId(lhs) < getIslandId(rhs);
+		}
+};
+
+
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects)
+{
+
+	BT_PROFILE("islandUnionFindAndQuickSort");
+	
+	m_islandmanifold.resize(0);
+
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+	
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+
+
+	//update the sleeping state for bodies, if all are sleeping
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+//				printf("error in island management\n");
+			}
+
+			assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState()== ACTIVE_TAG)
+				{
+					allSleeping = false;
+				}
+				if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+				}
+			}
+		}
+			
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState( ISLAND_SLEEPING );
+				}
+			}
+		} else
+		{
+
+			int idx;
+			for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+//					printf("error in island management\n");
+				}
+
+				assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+					}
+				}
+			}
+		}
+	}
+
+	
+	int i;
+	int maxNumManifolds = dispatcher->getNumManifolds();
+
+#define SPLIT_ISLANDS 1
+#ifdef SPLIT_ISLANDS
+
+	
+#endif //SPLIT_ISLANDS
+
+	
+	for (i=0;i<maxNumManifolds ;i++)
+	{
+		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		 
+		 btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
+		 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
+		
+		 //todo: check sleeping conditions!
+		 if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+		{
+		
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj1->activate();
+			}
+			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				colObj0->activate();
+			}
+#ifdef SPLIT_ISLANDS
+	//		//filtering for response
+			if (dispatcher->needsResponse(colObj0,colObj1))
+				m_islandmanifold.push_back(manifold);
+#endif //SPLIT_ISLANDS
+		}
+	}
+}
+
+
+
+//
+// todo: this is random access, it can be walked 'cache friendly'!
+//
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
+{
+
+	buildIslands(dispatcher,collisionObjects);
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+	int numElem = getUnionFind().getNumElements();
+
+	BT_PROFILE("processIslands");
+
+#ifndef SPLIT_ISLANDS
+	btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+	
+	callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+#else
+	// Sort manifolds, based on islands
+	// Sort the vector using predicate and std::sort
+	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+	int numManifolds = int (m_islandmanifold.size());
+
+	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+	m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+	//now process all active islands (sets of manifolds for now)
+
+	int startManifoldIndex = 0;
+	int endManifoldIndex = 1;
+
+	//int islandId;
+
+	
+
+//	printf("Start Islands\n");
+
+	//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+	       bool islandSleeping = false;
+                
+                for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+                {
+                        int i = getUnionFind().getElement(endIslandIndex).m_sz;
+                        btCollisionObject* colObj0 = collisionObjects[i];
+						m_islandBodies.push_back(colObj0);
+                        if (!colObj0->isActive())
+                                islandSleeping = true;
+                }
+                
+
+		//find the accompanying contact manifold for this islandId
+		int numIslandManifolds = 0;
+		btPersistentManifold** startManifold = 0;
+
+		if (startManifoldIndex<numManifolds)
+		{
+			int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+			if (curIslandId == islandId)
+			{
+				startManifold = &m_islandmanifold[startManifoldIndex];
+			
+				for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+				{
+
+				}
+				/// Process the actual simulation, only if not sleeping/deactivated
+				numIslandManifolds = endManifoldIndex-startManifoldIndex;
+			}
+
+		}
+
+		if (!islandSleeping)
+		{
+			callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+		}
+		
+		if (numIslandManifolds)
+		{
+			startManifoldIndex = endManifoldIndex;
+		}
+
+		m_islandBodies.resize(0);
+	}
+#endif //SPLIT_ISLANDS
+
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h	(revision 1963)
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SIMULATION_ISLAND_MANAGER_H
+#define SIMULATION_ISLAND_MANAGER_H
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+class btCollisionObject;
+class btCollisionWorld;
+class btDispatcher;
+class btPersistentManifold;
+
+
+///SimulationIslandManager creates and handles simulation islands, using btUnionFind
+class btSimulationIslandManager
+{
+	btUnionFind m_unionFind;
+
+	btAlignedObjectArray<btPersistentManifold*>  m_islandmanifold;
+	btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
+	
+	
+public:
+	btSimulationIslandManager();
+	virtual ~btSimulationIslandManager();
+
+
+	void initUnionFind(int n);	
+	
+		
+	btUnionFind& getUnionFind() { return m_unionFind;}
+
+	virtual	void	updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher);
+	virtual	void	storeIslandActivationState(btCollisionWorld* world);
+
+
+	void	findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+	
+
+	struct	IslandCallback
+	{
+		virtual ~IslandCallback() {};
+
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+	};
+
+	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);
+
+	void buildIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects);
+
+};
+
+#endif //SIMULATION_ISLAND_MANAGER_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,260 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+//#include <stdio.h>
+
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+	btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
+	btCollisionObject* boxObj = m_isSwapped? col0 : col1;
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
+		m_ownManifold = true;
+	}
+}
+
+
+btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+
+
+void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
+	btCollisionObject* boxObj = m_isSwapped? body0 : body1;
+
+
+	btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
+
+	btVector3 normalOnSurfaceB;
+	btVector3 pOnBox,pOnSphere;
+	btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
+	btScalar radius = sphere0->getRadius();
+	
+	btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	if (dist < SIMD_EPSILON)
+	{
+		btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
+
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+
+		resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
+		
+	}
+
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr->getNumContacts())
+		{
+			resultOut->refreshContactPoints();
+		}
+	}
+
+}
+
+btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
+
+
+btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) 
+{
+
+	btScalar margins;
+	btVector3 bounds[2];
+	btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
+	
+	bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
+	bounds[1] = boxShape->getHalfExtentsWithoutMargin();
+
+	margins = boxShape->getMargin();//also add sphereShape margin?
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	btVector3	boundsVec[2];
+	btScalar	fPenetration;
+
+	boundsVec[0] = bounds[0];
+	boundsVec[1] = bounds[1];
+
+	btVector3	marginsVec( margins, margins, margins );
+
+	// add margins
+	bounds[0] += marginsVec;
+	bounds[1] -= marginsVec;
+
+	/////////////////////////////////////////////////
+
+	btVector3	tmp, prel, n[6], normal, v3P;
+	btScalar   fSep = btScalar(10000000.0), fSepThis;
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+	
+	bool	bFound = false;
+
+	v3P = prel;
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3? 0:1;
+		if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
+		{
+			v3P = v3P - n[i]*fSepThis;		
+			bFound = true;
+		}
+	}
+	
+	//
+
+	if ( bFound )
+	{
+		bounds[0] = boundsVec[0];
+		bounds[1] = boundsVec[1];
+
+		normal = (prel - v3P).normalize();
+		pointOnBox = v3P + normal*margins;
+		v3PointOnSphere = prel - normal*fRadius;
+
+		if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
+		{
+			return btScalar(1.0);
+		}
+
+		// transform back in world space
+		tmp = m44T( pointOnBox);
+		pointOnBox    = tmp;
+		tmp  = m44T( v3PointOnSphere);		
+		v3PointOnSphere = tmp;
+		btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
+		
+		//if this fails, fallback into deeper penetration case, below
+		if (fSeps2 > SIMD_EPSILON)
+		{
+			fSep = - btSqrt(fSeps2);
+			normal = (pointOnBox-v3PointOnSphere);
+			normal *= btScalar(1.)/fSep;
+		}
+
+		return fSep;
+	}
+
+	//////////////////////////////////////////////////
+	// Deep penetration case
+
+	fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
+
+	bounds[0] = boundsVec[0];
+	bounds[1] = boundsVec[1];
+
+	if ( fPenetration <= btScalar(0.0) )
+		return (fPenetration-margins);
+	else
+		return btScalar(1.0);
+}
+
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) 
+{
+
+	btVector3 bounds[2];
+
+	bounds[0] = aabbMin;
+	bounds[1] = aabbMax;
+
+	btVector3	p0, tmp, prel, n[6], normal;
+	btScalar   fSep = btScalar(-10000000.0), fSepThis;
+
+	// set p0 and normal to a default value to shup up GCC
+	p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+
+	n[0].setValue( btScalar(-1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[1].setValue(  btScalar(0.0), btScalar(-1.0),  btScalar(0.0) );
+	n[2].setValue(  btScalar(0.0),  btScalar(0.0), btScalar(-1.0) );
+	n[3].setValue(  btScalar(1.0),  btScalar(0.0),  btScalar(0.0) );
+	n[4].setValue(  btScalar(0.0),  btScalar(1.0),  btScalar(0.0) );
+	n[5].setValue(  btScalar(0.0),  btScalar(0.0),  btScalar(1.0) );
+
+	const btTransform&	m44T = boxObj->getWorldTransform();
+
+	// convert  point in local space
+	prel = m44T.invXform( sphereCenter);
+
+	///////////
+
+	for (int i=0;i<6;i++)
+	{
+		int j = i<3 ? 0:1;
+		if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) )	return btScalar(1.0);
+		if ( fSepThis > fSep )
+		{
+			p0 = bounds[j];	normal = (btVector3&)n[i];
+			fSep = fSepThis;
+		}
+	}
+
+	pointOnBox = prel - normal*(normal.dot((prel-p0)));
+	v3PointOnSphere = pointOnBox + normal*fSep;
+
+	// transform back in world space
+	tmp  = m44T( pointOnBox);		
+	pointOnBox    = tmp;
+	tmp  = m44T( v3PointOnSphere);		v3PointOnSphere = tmp;
+	normal = (pointOnBox-v3PointOnSphere).normalize();
+
+	return fSep;
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
+#define SPHERE_BOX_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+
+/// btSphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_isSwapped;
+	
+public:
+
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btSphereBoxCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+	btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
+
+	btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //SPHERE_BOX_COLLISION_ALGORITHM_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,105 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSphereSphereCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+
+	if (!m_manifoldPtr)
+		return;
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
+	btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
+
+	btVector3 diff = col0->getWorldTransform().getOrigin()-  col1->getWorldTransform().getOrigin();
+	btScalar len = diff.length();
+	btScalar radius0 = sphere0->getRadius();
+	btScalar radius1 = sphere1->getRadius();
+
+#ifdef CLEAR_MANIFOLD
+	m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
+#endif
+
+	///iff distance positive, don't generate a new contact
+	if ( len > (radius0+radius1))
+	{
+#ifndef CLEAR_MANIFOLD
+		resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+		return;
+	}
+	///distance (negative means penetration)
+	btScalar dist = len - (radius0+radius1);
+
+	btVector3 normalOnSurfaceB(1,0,0);
+	if (len > SIMD_EPSILON)
+	{
+		normalOnSurfaceB = diff / len;
+	}
+
+	///point on A (worldspace)
+	btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+	///point on B (worldspace)
+	btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
+
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	
+	
+	resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
+
+#ifndef CLEAR_MANIFOLD
+	resultOut->refreshContactPoints();
+#endif //CLEAR_MANIFOLD
+
+}
+
+btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)col0;
+	(void)col1;
+	(void)dispatchInfo;
+	(void)resultOut;
+
+	//not yet
+	return btScalar(1.);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
+#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btCollisionDispatcher.h"
+
+class btPersistentManifold;
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+	
+	virtual ~btSphereSphereCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
+			return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSphereTriangleCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "SphereTriangleDetector.h"
+
+
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_swapped(swapped)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
+		m_ownManifold = true;
+	}
+}
+
+btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	btCollisionObject* sphereObj = m_swapped? col1 : col0;
+	btCollisionObject* triObj = m_swapped? col0 : col1;
+
+	btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
+	
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	SphereTriangleDetector detector(sphere,triangle);
+	
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = btScalar(1e30);//todo: tighter bounds
+	input.m_transformA = sphereObj->getWorldTransform();
+	input.m_transformB = triObj->getWorldTransform();
+
+	bool swapResults = m_swapped;
+
+	detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
+
+	if (m_ownManifold)
+		resultOut->refreshContactPoints();
+	
+}
+
+btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "btCollisionDispatcher.h"
+
+/// btSphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+/// Other features are frame-coherency (persistent data) and collision response.
+/// Also provides the most basic sample for custom/user btCollisionAlgorithm
+class btSphereTriangleCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_swapped;
+	
+public:
+	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
+
+	btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+	
+	virtual ~btSphereTriangleCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
+
+			return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
+		}
+	};
+
+};
+
+#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp	(revision 1963)
@@ -0,0 +1,83 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btUnionFind.h"
+#include <assert.h>
+
+
+
+
+btUnionFind::~btUnionFind()
+{
+	Free();
+
+}
+
+btUnionFind::btUnionFind()
+{ 
+
+}
+
+void	btUnionFind::allocate(int N)
+{
+	m_elements.resize(N);
+}
+void	btUnionFind::Free()
+{
+	m_elements.clear();
+}
+
+
+void	btUnionFind::reset(int N)
+{
+	allocate(N);
+
+	for (int i = 0; i < N; i++) 
+	{ 
+		m_elements[i].m_id = i; m_elements[i].m_sz = 1; 
+	} 
+}
+
+
+class btUnionFindElementSortPredicate
+{
+	public:
+
+		bool operator() ( const btElement& lhs, const btElement& rhs )
+		{
+			return lhs.m_id < rhs.m_id;
+		}
+};
+
+///this is a special operation, destroying the content of btUnionFind.
+///it sorts the elements, based on island id, in order to make it easy to iterate over islands
+void	btUnionFind::sortIslands()
+{
+
+	//first store the original body index, and islandId
+	int numElements = m_elements.size();
+	
+	for (int i=0;i<numElements;i++)
+	{
+		m_elements[i].m_id = find(i);
+		m_elements[i].m_sz = i;
+	}
+	
+	 // Sort the vector using predicate and std::sort
+	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	  m_elements.quickSort(btUnionFindElementSortPredicate());
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h	(revision 1963)
@@ -0,0 +1,124 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef UNION_FIND_H
+#define UNION_FIND_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+	#define USE_PATH_COMPRESSION 1
+
+struct	btElement
+{
+	int	m_id;
+	int	m_sz;
+};
+
+///UnionFind calculates connected subsets
+// Implements weighted Quick Union with path compression
+// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable)
+class btUnionFind
+  {
+    private:
+		btAlignedObjectArray<btElement>	m_elements;
+
+    public:
+	  
+		btUnionFind();
+		~btUnionFind();
+
+	
+		//this is a special operation, destroying the content of btUnionFind.
+		//it sorts the elements, based on island id, in order to make it easy to iterate over islands
+		void	sortIslands();
+
+	  void	reset(int N);
+
+	  SIMD_FORCE_INLINE int	getNumElements() const
+	  {
+		  return int(m_elements.size());
+	  }
+	  SIMD_FORCE_INLINE bool  isRoot(int x) const
+	  {
+		  return (x == m_elements[x].m_id);
+	  }
+
+	  btElement&	getElement(int index)
+	  {
+		  return m_elements[index];
+	  }
+	  const btElement& getElement(int index) const
+	  {
+		  return m_elements[index];
+	  }
+   
+	  void	allocate(int N);
+	  void	Free();
+
+
+
+
+	  int find(int p, int q)
+		{ 
+			return (find(p) == find(q)); 
+		}
+
+		void unite(int p, int q)
+		{
+			int i = find(p), j = find(q);
+			if (i == j) 
+				return;
+
+#ifndef USE_PATH_COMPRESSION
+			//weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+			if (m_elements[i].m_sz < m_elements[j].m_sz)
+			{ 
+				m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+			}
+			else 
+			{ 
+				m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; 
+			}
+#else
+			m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+#endif //USE_PATH_COMPRESSION
+		}
+
+		int find(int x)
+		{ 
+			//assert(x < m_N);
+			//assert(x >= 0);
+
+			while (x != m_elements[x].m_id) 
+			{
+		//not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+	
+		#ifdef USE_PATH_COMPRESSION
+				//
+				m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
+		#endif //
+				x = m_elements[x].m_id;
+				//assert(x < m_N);
+				//assert(x >= 0);
+
+			}
+			return x; 
+		}
+
+
+  };
+
+
+#endif //UNION_FIND_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.cpp	(revision 1963)
@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBoxShape.h"
+
+
+//{ 
+
+
+void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+
+
+void	btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	//btScalar margin = btScalar(0.);
+	btVector3 halfExtents = getHalfExtentsWithMargin();
+
+	btScalar lx=btScalar(2.)*(halfExtents.x());
+	btScalar ly=btScalar(2.)*(halfExtents.y());
+	btScalar lz=btScalar(2.)*(halfExtents.z());
+
+	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
+					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
+					mass/(btScalar(12.0)) * (lx*lx + ly*ly));
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h	(revision 1963)
@@ -0,0 +1,323 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OBB_BOX_MINKOWSKI_H
+#define OBB_BOX_MINKOWSKI_H
+
+#include "btPolyhedralConvexShape.h"
+#include "btCollisionMargin.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btMinMax.h"
+
+///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space.
+class btBoxShape: public btPolyhedralConvexShape
+{
+
+	//btVector3	m_boxHalfExtents1; //use m_implicitShapeDimensions instead
+
+
+public:
+
+	btVector3 getHalfExtentsWithMargin() const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
+		return halfExtents;
+	}
+	
+	const btVector3& getHalfExtentsWithoutMargin() const
+	{
+		return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included
+	}
+	
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
+		
+		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+	}
+
+	SIMD_FORCE_INLINE  btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	{
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+		
+		return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+			btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+			btFsels(vec.z(), halfExtents.z(), -halfExtents.z()));
+	}
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	{
+		const btVector3& halfExtents = getHalfExtentsWithoutMargin();
+	
+		for (int i=0;i<numVectors;i++)
+		{
+			const btVector3& vec = vectors[i];
+			supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()),
+				btFsels(vec.y(), halfExtents.y(), -halfExtents.y()),
+				btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); 
+		}
+
+	}
+
+
+	btBoxShape( const btVector3& boxHalfExtents) 
+		: btPolyhedralConvexShape()
+	{
+		m_shapeType = BOX_SHAPE_PROXYTYPE;
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+	};
+
+	virtual void setMargin(btScalar collisionMargin)
+	{
+		//correct the m_implicitShapeDimensions for the margin
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		
+		btConvexInternalShape::setMargin(collisionMargin);
+		btVector3 newMargin(getMargin(),getMargin(),getMargin());
+		m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin;
+
+	}
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		btVector3 oldMargin(getMargin(),getMargin(),getMargin());
+		btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin;
+		btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling;
+
+		btConvexInternalShape::setLocalScaling(scaling);
+
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin;
+
+	}
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const
+	{
+		//this plane might not be aligned...
+		btVector4 plane ;
+		getPlaneEquation(plane,i);
+		planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ());
+		planeSupport = localGetSupportingVertex(-planeNormal);
+	}
+
+	
+	virtual int getNumPlanes() const
+	{
+		return 6;
+	}	
+	
+	virtual int	getNumVertices() const 
+	{
+		return 8;
+	}
+
+	virtual int getNumEdges() const
+	{
+		return 12;
+	}
+
+
+	virtual void getVertex(int i,btVector3& vtx) const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+		vtx = btVector3(
+				halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1),
+				halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1),
+				halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2));
+	}
+	
+
+	virtual void	getPlaneEquation(btVector4& plane,int i) const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+		switch (i)
+		{
+		case 0:
+			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
+			plane[3] = -halfExtents.x();
+			break;
+		case 1:
+			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
+			plane[3] = -halfExtents.x();
+			break;
+		case 2:
+			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
+			plane[3] = -halfExtents.y();
+			break;
+		case 3:
+			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
+			plane[3] = -halfExtents.y();
+			break;
+		case 4:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
+			plane[3] = -halfExtents.z();
+			break;
+		case 5:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
+			plane[3] = -halfExtents.z();
+			break;
+		default:
+			assert(0);
+		}
+	}
+
+	
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const
+	//virtual void getEdge(int i,Edge& edge) const
+	{
+		int edgeVert0 = 0;
+		int edgeVert1 = 0;
+
+		switch (i)
+		{
+		case 0:
+				edgeVert0 = 0;
+				edgeVert1 = 1;
+			break;
+		case 1:
+				edgeVert0 = 0;
+				edgeVert1 = 2;
+			break;
+		case 2:
+			edgeVert0 = 1;
+			edgeVert1 = 3;
+
+			break;
+		case 3:
+			edgeVert0 = 2;
+			edgeVert1 = 3;
+			break;
+		case 4:
+			edgeVert0 = 0;
+			edgeVert1 = 4;
+			break;
+		case 5:
+			edgeVert0 = 1;
+			edgeVert1 = 5;
+
+			break;
+		case 6:
+			edgeVert0 = 2;
+			edgeVert1 = 6;
+			break;
+		case 7:
+			edgeVert0 = 3;
+			edgeVert1 = 7;
+			break;
+		case 8:
+			edgeVert0 = 4;
+			edgeVert1 = 5;
+			break;
+		case 9:
+			edgeVert0 = 4;
+			edgeVert1 = 6;
+			break;
+		case 10:
+			edgeVert0 = 5;
+			edgeVert1 = 7;
+			break;
+		case 11:
+			edgeVert0 = 6;
+			edgeVert1 = 7;
+			break;
+		default:
+			btAssert(0);
+
+		}
+
+		getVertex(edgeVert0,pa );
+		getVertex(edgeVert1,pb );
+	}
+
+
+
+
+	
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+
+		//btScalar minDist = 2*tolerance;
+		
+		bool result =	(pt.x() <= (halfExtents.x()+tolerance)) &&
+						(pt.x() >= (-halfExtents.x()-tolerance)) &&
+						(pt.y() <= (halfExtents.y()+tolerance)) &&
+						(pt.y() >= (-halfExtents.y()-tolerance)) &&
+						(pt.z() <= (halfExtents.z()+tolerance)) &&
+						(pt.z() >= (-halfExtents.z()-tolerance));
+		
+		return result;
+	}
+
+
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "Box";
+	}
+
+	virtual int		getNumPreferredPenetrationDirections() const
+	{
+		return 6;
+	}
+	
+	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+	{
+		switch (index)
+		{
+		case 0:
+			penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
+			break;
+		case 1:
+			penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
+			break;
+		case 2:
+			penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
+			break;
+		case 3:
+			penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
+			break;
+		case 4:
+			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
+			break;
+		case 5:
+			penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
+			break;
+		default:
+			assert(0);
+		}
+	}
+
+};
+
+#endif //OBB_BOX_MINKOWSKI_H
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp	(revision 1963)
@@ -0,0 +1,344 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//#define DISABLE_BVH
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh)
+:btTriangleMeshShape(meshInterface),
+m_bvh(0),
+m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+m_ownsBvh(false)
+{
+	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	//construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+	btVector3 bvhAabbMin,bvhAabbMax;
+	if(meshInterface->hasPremadeAabb())
+	{
+		meshInterface->getPremadeAabb(&bvhAabbMin, &bvhAabbMax);
+	}
+	else
+	{
+		meshInterface->calculateAabbBruteForce(bvhAabbMin,bvhAabbMax);
+	}
+	
+	if (buildBvh)
+	{
+		void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+		m_bvh = new (mem) btOptimizedBvh();
+		m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+		m_ownsBvh = true;
+	}
+
+#endif //DISABLE_BVH
+
+}
+
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh)
+:btTriangleMeshShape(meshInterface),
+m_bvh(0),
+m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+m_ownsBvh(false)
+{
+	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	//construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+	if (buildBvh)
+	{
+		void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+		m_bvh = new (mem) btOptimizedBvh();
+		
+		m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax);
+		m_ownsBvh = true;
+	}
+
+#endif //DISABLE_BVH
+
+}
+
+void	btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax );
+	
+	m_localAabbMin.setMin(aabbMin);
+	m_localAabbMax.setMax(aabbMax);
+}
+
+
+void	btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	m_bvh->refit( m_meshInterface, aabbMin,aabbMax );
+	
+	recalcLocalAabb();
+}
+
+btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
+{
+	if (m_ownsBvh)
+	{
+		m_bvh->~btOptimizedBvh();
+		btAlignedFree(m_bvh);
+	}
+}
+
+void	btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
+{
+	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	{
+		btStridingMeshInterface*	m_meshInterface;
+		btTriangleCallback* m_callback;
+
+		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+			:m_meshInterface(meshInterface),
+			m_callback(callback)
+		{
+		}
+				
+		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+		{
+			btVector3 m_triangle[3];
+			const unsigned char *vertexbase;
+			int numverts;
+			PHY_ScalarType type;
+			int stride;
+			const unsigned char *indexbase;
+			int indexstride;
+			int numfaces;
+			PHY_ScalarType indicestype;
+
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				nodeSubPart);
+
+			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+	
+			const btVector3& meshScaling = m_meshInterface->getScaling();
+			for (int j=2;j>=0;j--)
+			{
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+				btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
+
+				m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+			}
+
+			/* Perform ray vs. triangle collision here */
+			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+		}
+	};
+
+	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+
+	m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget);
+}
+
+void	btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
+{
+	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	{
+		btStridingMeshInterface*	m_meshInterface;
+		btTriangleCallback* m_callback;
+
+		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+			:m_meshInterface(meshInterface),
+			m_callback(callback)
+		{
+		}
+				
+		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+		{
+			btVector3 m_triangle[3];
+			const unsigned char *vertexbase;
+			int numverts;
+			PHY_ScalarType type;
+			int stride;
+			const unsigned char *indexbase;
+			int indexstride;
+			int numfaces;
+			PHY_ScalarType indicestype;
+
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				nodeSubPart);
+
+			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+	
+			const btVector3& meshScaling = m_meshInterface->getScaling();
+			for (int j=2;j>=0;j--)
+			{
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+				btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
+
+				m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+			}
+
+			/* Perform ray vs. triangle collision here */
+			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+		}
+	};
+
+	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+
+	m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
+}
+
+//perform bvh tree traversal and report overlapping triangles to 'callback'
+void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+
+#ifdef DISABLE_BVH
+	//brute force traverse all triangles
+	btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax);
+#else
+
+	//first get all the nodes
+
+	
+	struct	MyNodeOverlapCallback : public btNodeOverlapCallback
+	{
+		btStridingMeshInterface*	m_meshInterface;
+		btTriangleCallback*		m_callback;
+		btVector3				m_triangle[3];
+
+
+		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+			:m_meshInterface(meshInterface),
+			m_callback(callback)
+		{
+		}
+				
+		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+		{
+			const unsigned char *vertexbase;
+			int numverts;
+			PHY_ScalarType type;
+			int stride;
+			const unsigned char *indexbase;
+			int indexstride;
+			int numfaces;
+			PHY_ScalarType indicestype;
+			
+
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,
+				numverts,
+				type,
+				stride,
+				&indexbase,
+				indexstride,
+				numfaces,
+				indicestype,
+				nodeSubPart);
+
+			unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+			btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+	
+			const btVector3& meshScaling = m_meshInterface->getScaling();
+			for (int j=2;j>=0;j--)
+			{
+				
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+
+
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+				btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
+
+				m_triangle[j] = btVector3(
+					graphicsbase[0]*meshScaling.getX(),
+					graphicsbase[1]*meshScaling.getY(),
+					graphicsbase[2]*meshScaling.getZ());
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
+#endif //DEBUG_TRIANGLE_MESH
+			}
+
+			m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex);
+			m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
+		}
+
+	};
+
+	MyNodeOverlapCallback	myNodeCallback(callback,m_meshInterface);
+
+	m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+
+#endif//DISABLE_BVH
+
+
+}
+
+void   btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+   if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+   {
+      btTriangleMeshShape::setLocalScaling(scaling);
+      if (m_ownsBvh)
+      {
+         m_bvh->~btOptimizedBvh();
+         btAlignedFree(m_bvh);
+      }
+      ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
+      void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16);
+      m_bvh = new(mem) btOptimizedBvh();
+      //rebuild the bvh...
+      m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax);
+      m_ownsBvh = true;
+   }
+}
+
+void   btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
+{
+   btAssert(!m_bvh);
+   btAssert(!m_ownsBvh);
+
+   m_bvh = bvh;
+   m_ownsBvh = false;
+   // update the scaling without rebuilding the bvh
+   if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+   {
+      btTriangleMeshShape::setLocalScaling(scaling);
+   }
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h	(revision 1963)
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BVH_TRIANGLE_MESH_SHAPE_H
+#define BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "btTriangleMeshShape.h"
+#include "btOptimizedBvh.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+///The btBvhTriangleMeshShape is a static-triangle mesh shape with several optimizations, such as bounding volume hierarchy and cache friendly traversal for PlayStation 3 Cell SPU. It is recommended to enable useQuantizedAabbCompression for better memory usage.
+///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method.
+///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk.
+///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example.
+ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
+{
+
+	btOptimizedBvh*	m_bvh;
+	bool m_useQuantizedAabbCompression;
+	bool m_ownsBvh;
+	bool m_pad[11];////need padding due to alignment
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btBvhTriangleMeshShape() : btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;};
+	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
+
+	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
+	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true);
+	
+	virtual ~btBvhTriangleMeshShape();
+
+	bool getOwnsBvh () const
+	{
+		return m_ownsBvh;
+	}
+
+
+	
+	void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget);
+	void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax);
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	void	refitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+
+	///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks
+	void	partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax);
+
+	//debugging
+	virtual const char*	getName()const {return "BVHTRIANGLEMESH";}
+
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	
+	btOptimizedBvh*	getOptimizedBvh()
+	{
+		return m_bvh;
+	}
+
+
+	void	setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1));
+
+	bool	usesQuantizedAabbCompression() const
+	{
+		return	m_useQuantizedAabbCompression;
+	}
+}
+;
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.cpp	(revision 1963)
@@ -0,0 +1,171 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btCapsuleShape.h"
+
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btQuaternion.h"
+
+btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
+{
+	m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
+	m_upAxis = 1;
+	m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
+}
+
+ 
+ btVector3	btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+
+	btVector3 supVec(0,0,0);
+
+	btScalar maxDot(btScalar(-1e30));
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	btVector3 vtx;
+	btScalar newDot;
+	
+	btScalar radius = getRadius();
+
+
+	{
+		btVector3 pos(0,0,0);
+		pos[getUpAxis()] = getHalfHeight();
+
+		vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+	{
+		btVector3 pos(0,0,0);
+		pos[getUpAxis()] = -getHalfHeight();
+
+		vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+
+	return supVec;
+
+}
+
+ void	btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+
+	
+	btScalar radius = getRadius();
+
+	for (int j=0;j<numVectors;j++)
+	{
+		btScalar maxDot(btScalar(-1e30));
+		const btVector3& vec = vectors[j];
+
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[getUpAxis()] = getHalfHeight();
+			vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[j] = vtx;
+			}
+		}
+		{
+			btVector3 pos(0,0,0);
+			pos[getUpAxis()] = -getHalfHeight();
+			vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[j] = vtx;
+			}
+		}
+		
+	}
+}
+
+
+void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	//as an approximation, take the inertia of the box that bounds the spheres
+
+	btTransform ident;
+	ident.setIdentity();
+
+	
+	btScalar radius = getRadius();
+
+	btVector3 halfExtents(radius,radius,radius);
+	halfExtents[getUpAxis()]+=getHalfHeight();
+
+	btScalar margin = CONVEX_DISTANCE_MARGIN;
+
+	btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
+	btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
+	btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(.08333333);
+
+	inertia[0] = scaledmass * (y2+z2);
+	inertia[1] = scaledmass * (x2+z2);
+	inertia[2] = scaledmass * (x2+y2);
+
+}
+
+btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
+{
+	m_upAxis = 0;
+	m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
+}
+
+
+
+
+
+
+btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
+{
+	m_upAxis = 2;
+	m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
+}
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h	(revision 1963)
@@ -0,0 +1,118 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CAPSULE_SHAPE_H
+#define BT_CAPSULE_SHAPE_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+
+///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
+///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
+class btCapsuleShape : public btConvexInternalShape
+{
+protected:
+	int	m_upAxis;
+
+protected:
+	///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
+	btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
+
+public:
+	btCapsuleShape(btScalar radius,btScalar height);
+
+	///CollisionShape Interface
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	/// btConvexShape Interface
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	
+	virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+	{
+			btVector3 halfExtents(getRadius(),getRadius(),getRadius());
+			halfExtents[m_upAxis] = getRadius() + getHalfHeight();
+			halfExtents += btVector3(getMargin(),getMargin(),getMargin());
+			btMatrix3x3 abs_b = t.getBasis().absolute();  
+			btPoint3 center = t.getOrigin();
+			btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));		  
+			
+			aabbMin = center - extent;
+			aabbMax = center + extent;
+	}
+
+	virtual const char*	getName()const 
+	{
+		return "CapsuleShape";
+	}
+
+	int	getUpAxis() const
+	{
+		return m_upAxis;
+	}
+
+	btScalar	getRadius() const
+	{
+		int radiusAxis = (m_upAxis+2)%3;
+		return m_implicitShapeDimensions[radiusAxis];
+	}
+
+	btScalar	getHalfHeight() const
+	{
+		return m_implicitShapeDimensions[m_upAxis];
+	}
+
+};
+
+///btCapsuleShapeX represents a capsule around the Z axis
+///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+class btCapsuleShapeX : public btCapsuleShape
+{
+public:
+
+	btCapsuleShapeX(btScalar radius,btScalar height);
+		
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "CapsuleX";
+	}
+
+	
+
+};
+
+///btCapsuleShapeZ represents a capsule around the Z axis
+///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
+class btCapsuleShapeZ : public btCapsuleShape
+{
+public:
+	btCapsuleShapeZ(btScalar radius,btScalar height);
+
+		//debugging
+	virtual const char*	getName()const
+	{
+		return "CapsuleZ";
+	}
+
+	
+};
+
+
+
+#endif //BT_CAPSULE_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionMargin.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionMargin.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionMargin.h	(revision 1963)
@@ -0,0 +1,26 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_MARGIN_H
+#define COLLISION_MARGIN_H
+
+//used by Gjk and some other algorithms
+
+#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)
+
+
+
+#endif //COLLISION_MARGIN_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp	(revision 1963)
@@ -0,0 +1,90 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C" 
+{
+void btBulletCollisionProbe ();
+
+void btBulletCollisionProbe () {}
+}
+
+
+
+void	btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const
+{
+	btTransform tr;
+	tr.setIdentity();
+	btVector3 aabbMin,aabbMax;
+
+	getAabb(tr,aabbMin,aabbMax);
+
+	radius = (aabbMax-aabbMin).length()*btScalar(0.5);
+	center = (aabbMin+aabbMax)*btScalar(0.5);
+}
+
+btScalar	btCollisionShape::getAngularMotionDisc() const
+{
+	btVector3	center;
+	btScalar disc;
+	getBoundingSphere(center,disc);
+	disc += (center).length();
+	return disc;
+}
+
+void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
+{
+	//start with static aabb
+	getAabb(curTrans,temporalAabbMin,temporalAabbMax);
+
+	btScalar temporalAabbMaxx = temporalAabbMax.getX();
+	btScalar temporalAabbMaxy = temporalAabbMax.getY();
+	btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+	btScalar temporalAabbMinx = temporalAabbMin.getX();
+	btScalar temporalAabbMiny = temporalAabbMin.getY();
+	btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+	// add linear motion
+	btVector3 linMotion = linvel*timeStep;
+	//todo: simd would have a vector max/min operation, instead of per-element access
+	if (linMotion.x() > btScalar(0.))
+		temporalAabbMaxx += linMotion.x(); 
+	else
+		temporalAabbMinx += linMotion.x();
+	if (linMotion.y() > btScalar(0.))
+		temporalAabbMaxy += linMotion.y(); 
+	else
+		temporalAabbMiny += linMotion.y();
+	if (linMotion.z() > btScalar(0.))
+		temporalAabbMaxz += linMotion.z(); 
+	else
+		temporalAabbMinz += linMotion.z();
+
+	//add conservative angular motion
+	btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
+	btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+	temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+	temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+	temporalAabbMin -= angularMotion3d;
+	temporalAabbMax += angularMotion3d;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.h	(revision 1963)
@@ -0,0 +1,111 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLISION_SHAPE_H
+#define COLLISION_SHAPE_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btPoint3.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
+
+///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
+class btCollisionShape
+{
+protected:
+	int m_shapeType;
+	void* m_userPointer;
+
+public:
+
+	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0)
+	{
+	}
+
+	virtual ~btCollisionShape()
+	{
+	}
+
+	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+	virtual void	getBoundingSphere(btVector3& center,btScalar& radius) const;
+
+	///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
+	virtual btScalar	getAngularMotionDisc() const;
+
+
+	///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
+	///result is conservative
+	void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const;
+
+#ifndef __SPU__
+
+	SIMD_FORCE_INLINE bool	isPolyhedral() const
+	{
+		return btBroadphaseProxy::isPolyhedral(getShapeType());
+	}
+
+	SIMD_FORCE_INLINE bool	isConvex() const
+	{
+		return btBroadphaseProxy::isConvex(getShapeType());
+	}
+	SIMD_FORCE_INLINE bool	isConcave() const
+	{
+		return btBroadphaseProxy::isConcave(getShapeType());
+	}
+	SIMD_FORCE_INLINE bool	isCompound() const
+	{
+		return btBroadphaseProxy::isCompound(getShapeType());
+	}
+
+	///isInfinite is used to catch simulation error (aabb check)
+	SIMD_FORCE_INLINE bool isInfinite() const
+	{
+		return btBroadphaseProxy::isInfinite(getShapeType());
+	}
+
+	
+	virtual void	setLocalScaling(const btVector3& scaling) =0;
+	virtual const btVector3& getLocalScaling() const =0;
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0;
+
+
+//debugging support
+	virtual const char*	getName()const =0 ;
+#endif //__SPU__
+
+	
+	int		getShapeType() const { return m_shapeType; }
+	virtual void	setMargin(btScalar margin) = 0;
+	virtual btScalar	getMargin() const = 0;
+
+	
+	///optional user data pointer
+	void	setUserPointer(void*  userPtr)
+	{
+		m_userPointer = userPtr;
+	}
+
+	void*	getUserPointer() const
+	{
+		return m_userPointer;
+	}
+
+};	
+
+#endif //COLLISION_SHAPE_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp	(revision 1963)
@@ -0,0 +1,232 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btCompoundShape.h"
+#include "btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+
+btCompoundShape::btCompoundShape()
+: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
+m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
+m_collisionMargin(btScalar(0.)),
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_dynamicAabbTree(0)
+{
+	m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
+	void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+	m_dynamicAabbTree = new(mem) btDbvt();
+	btAssert(mem==m_dynamicAabbTree);
+}
+
+
+btCompoundShape::~btCompoundShape()
+{
+	if (m_dynamicAabbTree)
+	{
+		m_dynamicAabbTree->~btDbvt();
+		btAlignedFree(m_dynamicAabbTree);
+	}
+}
+
+void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+{
+	//m_childTransforms.push_back(localTransform);
+	//m_childShapes.push_back(shape);
+	btCompoundShapeChild child;
+	child.m_transform = localTransform;
+	child.m_childShape = shape;
+	child.m_childShapeType = shape->getShapeType();
+	child.m_childMargin = shape->getMargin();
+
+	m_children.push_back(child);
+
+	//extend the local aabbMin/aabbMax
+	btVector3 localAabbMin,localAabbMax;
+	shape->getAabb(localTransform,localAabbMin,localAabbMax);
+	for (int i=0;i<3;i++)
+	{
+		if (m_localAabbMin[i] > localAabbMin[i])
+		{
+			m_localAabbMin[i] = localAabbMin[i];
+		}
+		if (m_localAabbMax[i] < localAabbMax[i])
+		{
+			m_localAabbMax[i] = localAabbMax[i];
+		}
+
+	}
+	if (m_dynamicAabbTree)
+	{
+		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		int index = m_children.size()-1;
+		child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+	}
+
+}
+
+void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
+{
+	btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
+	if (m_dynamicAabbTree)
+	{
+		m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
+	}
+	m_children.swap(childShapeIndex,m_children.size()-1);
+	m_children.pop_back();
+
+}
+
+void btCompoundShape::removeChildShape(btCollisionShape* shape)
+{
+	// Find the children containing the shape specified, and remove those children.
+	//note: there might be multiple children using the same shape!
+	for(int i = m_children.size()-1; i >= 0 ; i--)
+	{
+		if(m_children[i].m_childShape == shape)
+		{
+			m_children.swap(i,m_children.size()-1);
+			m_children.pop_back();
+			//remove it from the m_dynamicAabbTree too
+			//m_dynamicAabbTree->remove(m_aabbProxies[i]);
+			//m_aabbProxies.swap(i,m_children.size()-1);
+			//m_aabbProxies.pop_back();
+		}
+	}
+
+
+
+	recalculateLocalAabb();
+}
+
+void btCompoundShape::recalculateLocalAabb()
+{
+	// Recalculate the local aabb
+	// Brute force, it iterates over all the shapes left.
+	m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+
+	//extend the local aabbMin/aabbMax
+	for (int j = 0; j < m_children.size(); j++)
+	{
+		btVector3 localAabbMin,localAabbMax;
+		m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
+		for (int i=0;i<3;i++)
+		{
+			if (m_localAabbMin[i] > localAabbMin[i])
+				m_localAabbMin[i] = localAabbMin[i];
+			if (m_localAabbMax[i] < localAabbMax[i])
+				m_localAabbMax[i] = localAabbMax[i];
+		}
+	}
+}
+
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
+	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+
+	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+	btPoint3 center = trans(localCenter);
+
+	btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+		abs_b[1].dot(localHalfExtents),
+		abs_b[2].dot(localHalfExtents));
+	aabbMin = center-extent;
+	aabbMax = center+extent;
+
+}
+
+void	btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	//approximation: take the inertia from the aabb for now
+	btTransform ident;
+	ident.setIdentity();
+	btVector3 aabbMin,aabbMax;
+	getAabb(ident,aabbMin,aabbMax);
+
+	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+	btScalar lx=btScalar(2.)*(halfExtents.x());
+	btScalar ly=btScalar(2.)*(halfExtents.y());
+	btScalar lz=btScalar(2.)*(halfExtents.z());
+
+	inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
+	inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
+	inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
+
+}
+
+
+
+
+void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+{
+	int n = m_children.size();
+
+	btScalar totalMass = 0;
+	btVector3 center(0, 0, 0);
+	for (int k = 0; k < n; k++)
+	{
+		center += m_children[k].m_transform.getOrigin() * masses[k];
+		totalMass += masses[k];
+	}
+	center /= totalMass;
+	principal.setOrigin(center);
+
+	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	for (int k = 0; k < n; k++)
+	{
+		btVector3 i;
+		m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
+
+		const btTransform& t = m_children[k].m_transform;
+		btVector3 o = t.getOrigin() - center;
+
+		//compute inertia tensor in coordinate system of compound shape
+		btMatrix3x3 j = t.getBasis().transpose();
+		j[0] *= i[0];
+		j[1] *= i[1];
+		j[2] *= i[2];
+		j = t.getBasis() * j;
+
+		//add inertia tensor
+		tensor[0] += j[0];
+		tensor[1] += j[1];
+		tensor[2] += j[2];
+
+		//compute inertia tensor of pointmass at o
+		btScalar o2 = o.length2();
+		j[0].setValue(o2, 0, 0);
+		j[1].setValue(0, o2, 0);
+		j[2].setValue(0, 0, o2);
+		j[0] += o * -o.x(); 
+		j[1] += o * -o.y(); 
+		j[2] += o * -o.z();
+
+		//add inertia tensor of pointmass
+		tensor[0] += masses[k] * j[0];
+		tensor[1] += masses[k] * j[1];
+		tensor[2] += masses[k] * j[2];
+	}
+
+	tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+	inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h	(revision 1963)
@@ -0,0 +1,162 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COMPOUND_SHAPE_H
+#define COMPOUND_SHAPE_H
+
+#include "btCollisionShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+//class btOptimizedBvh;
+struct btDbvt;
+
+ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btTransform			m_transform;
+	btCollisionShape*	m_childShape;
+	int					m_childShapeType;
+	btScalar			m_childMargin;
+	struct btDbvtNode*	m_node;
+};
+
+SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
+{
+	return  ( c1.m_transform      == c2.m_transform &&
+		c1.m_childShape     == c2.m_childShape &&
+		c1.m_childShapeType == c2.m_childShapeType &&
+		c1.m_childMargin    == c2.m_childMargin );
+}
+
+/// btCompoundShape allows to store multiple other btCollisionShapes
+/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
+ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
+{
+	//btAlignedObjectArray<btTransform>		m_childTransforms;
+	//btAlignedObjectArray<btCollisionShape*>	m_childShapes;
+	btAlignedObjectArray<btCompoundShapeChild> m_children;
+	btVector3						m_localAabbMin;
+	btVector3						m_localAabbMax;
+
+	//btOptimizedBvh*					m_aabbTree;
+	btDbvt*							m_dynamicAabbTree;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btCompoundShape();
+
+	virtual ~btCompoundShape();
+
+	void	addChildShape(const btTransform& localTransform,btCollisionShape* shape);
+
+	/// Remove all children shapes that contain the specified shape
+	virtual void removeChildShape(btCollisionShape* shape);
+
+	void removeChildShapeByIndex(int childShapeindex);
+
+
+	int		getNumChildShapes() const
+	{
+		return int (m_children.size());
+	}
+
+	btCollisionShape* getChildShape(int index)
+	{
+		return m_children[index].m_childShape;
+	}
+	const btCollisionShape* getChildShape(int index) const
+	{
+		return m_children[index].m_childShape;
+	}
+
+	btTransform	getChildTransform(int index)
+	{
+		return m_children[index].m_transform;
+	}
+	const btTransform	getChildTransform(int index) const
+	{
+		return m_children[index].m_transform;
+	}
+
+
+	btCompoundShapeChild* getChildList()
+	{
+		return &m_children[0];
+	}
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	virtual	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	/** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
+	Use this yourself if you modify the children or their transforms. */
+	virtual void recalculateLocalAabb(); 
+
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		m_localScaling = scaling;
+	}
+	virtual const btVector3& getLocalScaling() const 
+	{
+		return m_localScaling;
+	}
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void	setMargin(btScalar margin)
+	{
+		m_collisionMargin = margin;
+	}
+	virtual btScalar	getMargin() const
+	{
+		return m_collisionMargin;
+	}
+	virtual const char*	getName()const
+	{
+		return "Compound";
+	}
+
+	//this is optional, but should make collision queries faster, by culling non-overlapping nodes
+	void	createAabbTreeFromChildren();
+
+	btDbvt*							getDynamicAabbTree()
+	{
+		return m_dynamicAabbTree;
+	}
+
+	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+	///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
+	///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
+	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+	///of the collision object by the principal transform.
+	void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+
+
+private:
+	btScalar	m_collisionMargin;
+protected:
+	btVector3	m_localScaling;
+
+};
+
+
+
+#endif //COMPOUND_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.cpp	(revision 1963)
@@ -0,0 +1,28 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btConcaveShape.h"
+
+btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.))
+{
+
+}
+
+btConcaveShape::~btConcaveShape()
+{
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConcaveShape.h	(revision 1963)
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONCAVE_SHAPE_H
+#define CONCAVE_SHAPE_H
+
+#include "btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "btTriangleCallback.h"
+
+
+///The btConcaveShape class provides an interface for non-moving (static) concave shapes.
+///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape.
+class btConcaveShape : public btCollisionShape
+{
+protected:
+	btScalar m_collisionMargin;
+
+public:
+	btConcaveShape();
+
+	virtual ~btConcaveShape();
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0;
+
+	virtual btScalar getMargin() const {
+		return m_collisionMargin;
+	}
+	virtual void setMargin(btScalar collisionMargin)
+	{
+		m_collisionMargin = collisionMargin;
+	}
+
+
+
+};
+
+#endif //CONCAVE_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp	(revision 1963)
@@ -0,0 +1,134 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConeShape.h"
+#include "LinearMath/btPoint3.h"
+
+
+
+btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
+m_radius (radius),
+m_height(height)
+{
+	m_shapeType = CONE_SHAPE_PROXYTYPE;
+	setConeUpIndex(1);
+	btVector3 halfExtents;
+	m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
+}
+
+btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height):
+btConeShape(radius,height)
+{
+	setConeUpIndex(2);
+}
+
+btConeShapeX::btConeShapeX (btScalar radius,btScalar height):
+btConeShape(radius,height)
+{
+	setConeUpIndex(0);
+}
+
+///choose upAxis index
+void	btConeShape::setConeUpIndex(int upIndex)
+{
+	switch (upIndex)
+	{
+	case 0:
+			m_coneIndices[0] = 1;
+			m_coneIndices[1] = 0;
+			m_coneIndices[2] = 2;
+		break;
+	case 1:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 1;
+			m_coneIndices[2] = 2;
+		break;
+	case 2:
+			m_coneIndices[0] = 0;
+			m_coneIndices[1] = 2;
+			m_coneIndices[2] = 1;
+		break;
+	default:
+		assert(0);
+	};
+}
+
+btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
+{
+	
+	btScalar halfHeight = m_height * btScalar(0.5);
+
+ if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
+ {
+	btVector3 tmp;
+
+	tmp[m_coneIndices[0]] = btScalar(0.);
+	tmp[m_coneIndices[1]] = halfHeight;
+	tmp[m_coneIndices[2]] = btScalar(0.);
+	return tmp;
+ }
+  else {
+    btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
+    if (s > SIMD_EPSILON) {
+      btScalar d = m_radius / s;
+	  btVector3 tmp;
+	  tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
+	  tmp[m_coneIndices[1]] = -halfHeight;
+	  tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
+	  return tmp;
+    }
+    else  {
+		btVector3 tmp;
+		tmp[m_coneIndices[0]] = btScalar(0.);
+		tmp[m_coneIndices[1]] = -halfHeight;
+		tmp[m_coneIndices[2]] = btScalar(0.);
+		return tmp;
+	}
+  }
+
+}
+
+btVector3	btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
+{
+		return coneLocalSupport(vec);
+}
+
+void	btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		const btVector3& vec = vectors[i];
+		supportVerticesOut[i] = coneLocalSupport(vec);
+	}
+}
+
+
+btVector3	btConeShape::localGetSupportingVertex(const btVector3& vec)  const
+{
+	btVector3 supVertex = coneLocalSupport(vec);
+	if ( getMargin()!=btScalar(0.) )
+	{
+		btVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		} 
+		vecnorm.normalize();
+		supVertex+= getMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConeShape.h	(revision 1963)
@@ -0,0 +1,100 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONE_MINKOWSKI_H
+#define CONE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis.
+class btConeShape : public btConvexInternalShape
+
+{
+
+	btScalar m_sinAngle;
+	btScalar m_radius;
+	btScalar m_height;
+	int		m_coneIndices[3];
+	btVector3 coneLocalSupport(const btVector3& v) const;
+
+
+public:
+	btConeShape (btScalar radius,btScalar height);
+	
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const;
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec) const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+	btScalar getRadius() const { return m_radius;}
+	btScalar getHeight() const { return m_height;}
+
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
+	{
+		btTransform identity;
+		identity.setIdentity();
+		btVector3 aabbMin,aabbMax;
+		getAabb(identity,aabbMin,aabbMax);
+
+		btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+		btScalar margin = getMargin();
+
+		btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
+		btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
+		btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
+		const btScalar x2 = lx*lx;
+		const btScalar y2 = ly*ly;
+		const btScalar z2 = lz*lz;
+		const btScalar scaledmass = mass * btScalar(0.08333333);
+
+		inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+//		inertia.x() = scaledmass * (y2+z2);
+//		inertia.y() = scaledmass * (x2+z2);
+//		inertia.z() = scaledmass * (x2+y2);
+	}
+
+
+		virtual const char*	getName()const 
+		{
+			return "Cone";
+		}
+		
+		///choose upAxis index
+		void	setConeUpIndex(int upIndex);
+		
+		int	getConeUpIndex() const
+		{
+			return m_coneIndices[1];
+		}
+};
+
+///btConeShape implements a Cone shape, around the X axis
+class btConeShapeX : public btConeShape
+{
+	public:
+		btConeShapeX(btScalar radius,btScalar height);
+};
+
+///btConeShapeZ implements a Cone shape, around the Z axis
+class btConeShapeZ : public btConeShape
+{
+	public:
+		btConeShapeZ(btScalar radius,btScalar height);
+};
+#endif //CONE_MINKOWSKI_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp	(revision 1963)
@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+
+
+
+btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexShape ()
+{
+	m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE;
+	m_points.resize(numPoints);
+
+	unsigned char* pointsBaseAddress = (unsigned char*)points;
+
+	for (int i=0;i<numPoints;i++)
+	{
+		btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride);
+		m_points[i] = point[0];
+	}
+
+	recalcLocalAabb();
+
+}
+
+
+
+void btConvexHullShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling;
+	recalcLocalAabb();
+}
+
+void btConvexHullShape::addPoint(const btPoint3& point)
+{
+	m_points.push_back(point);
+	recalcLocalAabb();
+
+}
+
+btVector3	btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btScalar newDot,maxDot = btScalar(-1e30);
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+
+	for (int i=0;i<m_points.size();i++)
+	{
+		btPoint3 vtx = m_points[i] * m_localScaling;
+
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+	return supVec;
+}
+
+void	btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	btScalar newDot;
+	//use 'w' component of supportVerticesOut?
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			supportVerticesOut[i][3] = btScalar(-1e30);
+		}
+	}
+	for (int i=0;i<m_points.size();i++)
+	{
+		btPoint3 vtx = m_points[i] * m_localScaling;
+
+		for (int j=0;j<numVectors;j++)
+		{
+			const btVector3& vec = vectors[j];
+			
+			newDot = vec.dot(vtx);
+			if (newDot > supportVerticesOut[j][3])
+			{
+				//WARNING: don't swap next lines, the w component would get overwritten!
+				supportVerticesOut[j] = vtx;
+				supportVerticesOut[j][3] = newDot;
+			}
+		}
+	}
+
+
+
+}
+	
+
+
+btVector3	btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
+{
+	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+	if ( getMargin()!=btScalar(0.) )
+	{
+		btVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		} 
+		vecnorm.normalize();
+		supVertex+= getMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+
+
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
+int	btConvexHullShape::getNumVertices() const
+{
+	return m_points.size();
+}
+
+int btConvexHullShape::getNumEdges() const
+{
+	return m_points.size();
+}
+
+void btConvexHullShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const
+{
+
+	int index0 = i%m_points.size();
+	int index1 = (i+1)%m_points.size();
+	pa = m_points[index0]*m_localScaling;
+	pb = m_points[index1]*m_localScaling;
+}
+
+void btConvexHullShape::getVertex(int i,btPoint3& vtx) const
+{
+	vtx = m_points[i]*m_localScaling;
+}
+
+int	btConvexHullShape::getNumPlanes() const
+{
+	return 0;
+}
+
+void btConvexHullShape::getPlane(btVector3& ,btPoint3& ,int ) const
+{
+
+	btAssert(0);
+}
+
+//not yet
+bool btConvexHullShape::isInside(const btPoint3& ,btScalar ) const
+{
+	assert(0);
+	return false;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.h	(revision 1963)
@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_HULL_SHAPE_H
+#define CONVEX_HULL_SHAPE_H
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btAlignedObjectArray.h"
+
+///The btConvexHullShape implements an implicit convex hull of an array of vertices.
+///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex.
+ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape
+{
+	btAlignedObjectArray<btPoint3>	m_points;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	
+	///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory.
+	///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
+	///btConvexHullShape make an internal copy of the points.
+	btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btPoint3));
+
+	void addPoint(const btPoint3& point);
+
+	btPoint3* getPoints()
+	{
+		return &m_points[0];
+	}
+
+	const btPoint3* getPoints() const
+	{
+		return &m_points[0];
+	}
+
+	int getNumPoints() const 
+	{
+		return m_points.size();
+	}
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	
+
+
+	//debugging
+	virtual const char*	getName()const {return "Convex";}
+
+	
+	virtual int	getNumVertices() const;
+	virtual int getNumEdges() const;
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
+	virtual void getVertex(int i,btPoint3& vtx) const;
+	virtual int	getNumPlanes() const;
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const;
+
+	///in case we receive negative scaling
+	virtual void	setLocalScaling(const btVector3& scaling);
+
+};
+
+
+#endif //CONVEX_HULL_SHAPE_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.cpp	(revision 1963)
@@ -0,0 +1,78 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btConvexInternalShape.h"
+
+
+btConvexInternalShape::btConvexInternalShape()
+: btConvexShape (), m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_collisionMargin(CONVEX_DISTANCE_MARGIN)
+{
+}
+
+
+void	btConvexInternalShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling.absolute();
+}
+
+
+
+void	btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const
+{
+
+	btScalar margin = getMargin();
+	for (int i=0;i<3;i++)
+	{
+		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		vec[i] = btScalar(1.);
+
+		btVector3 sv = localGetSupportingVertex(vec*trans.getBasis());
+
+		btVector3 tmp = trans(sv);
+		maxAabb[i] = tmp[i]+margin;
+		vec[i] = btScalar(-1.);
+		tmp = trans(localGetSupportingVertex(vec*trans.getBasis()));
+		minAabb[i] = tmp[i]-margin;
+	}
+};
+
+
+btVector3	btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const
+{
+#ifndef __SPU__
+
+	 btVector3	supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+	if ( getMargin()!=btScalar(0.) )
+	{
+		btVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		} 
+		vecnorm.normalize();
+		supVertex+= getMargin() * vecnorm;
+	}
+	return supVertex;
+
+#else
+	return btVector3(0,0,0);
+#endif //__SPU__
+
+ }
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexInternalShape.h	(revision 1963)
@@ -0,0 +1,100 @@
+
+#ifndef BT_CONVEX_INTERNAL_SHAPE_H
+#define BT_CONVEX_INTERNAL_SHAPE_H
+
+#include "btConvexShape.h"
+
+///The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
+class btConvexInternalShape : public btConvexShape
+{
+
+	protected:
+
+	//local scaling. collisionMargin is not scaled !
+	btVector3	m_localScaling;
+
+	btVector3	m_implicitShapeDimensions;
+	
+	btScalar	m_collisionMargin;
+
+	btScalar	m_padding;
+
+	btConvexInternalShape();
+
+public:
+
+	
+
+	virtual ~btConvexInternalShape()
+	{
+
+	}
+
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+#ifndef __SPU__
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
+	
+	//notice that the vectors should be unit length
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
+#endif //#ifndef __SPU__
+
+	const btVector3& getImplicitShapeDimensions() const
+	{
+		return m_implicitShapeDimensions;
+	}
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		getAabbSlow(t,aabbMin,aabbMax);
+	}
+
+
+	
+	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const 
+	{
+		return m_localScaling;
+	}
+
+	const btVector3& getLocalScalingNV() const 
+	{
+		return m_localScaling;
+	}
+
+	virtual void	setMargin(btScalar margin)
+	{
+		m_collisionMargin = margin;
+	}
+	virtual btScalar	getMargin() const
+	{
+		return m_collisionMargin;
+	}
+
+	btScalar	getMarginNV() const
+	{
+		return m_collisionMargin;
+	}
+
+	virtual int		getNumPreferredPenetrationDirections() const
+	{
+		return 0;
+	}
+	
+	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+	{
+		(void)penetrationVector;
+		(void)index;
+		btAssert(0);
+	}
+
+
+	
+};
+
+
+#endif //BT_CONVEX_INTERNAL_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp	(revision 1963)
@@ -0,0 +1,156 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btConvexPointCloudShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+
+void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling;
+	recalcLocalAabb();
+}
+
+#ifndef __SPU__
+btVector3	btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btScalar newDot,maxDot = btScalar(-1e30);
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+
+	for (int i=0;i<m_numPoints;i++)
+	{
+		btPoint3 vtx = m_points[i] * m_localScaling;
+
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+	return supVec;
+}
+
+void	btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	btScalar newDot;
+	//use 'w' component of supportVerticesOut?
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			supportVerticesOut[i][3] = btScalar(-1e30);
+		}
+	}
+	for (int i=0;i<m_numPoints;i++)
+	{
+		btPoint3 vtx = m_points[i] * m_localScaling;
+
+		for (int j=0;j<numVectors;j++)
+		{
+			const btVector3& vec = vectors[j];
+			
+			newDot = vec.dot(vtx);
+			if (newDot > supportVerticesOut[j][3])
+			{
+				//WARNING: don't swap next lines, the w component would get overwritten!
+				supportVerticesOut[j] = vtx;
+				supportVerticesOut[j][3] = newDot;
+			}
+		}
+	}
+
+
+
+}
+	
+
+
+btVector3	btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const
+{
+	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+	if ( getMargin()!=btScalar(0.) )
+	{
+		btVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		} 
+		vecnorm.normalize();
+		supVertex+= getMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+
+#endif
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
+int	btConvexPointCloudShape::getNumVertices() const
+{
+	return m_numPoints;
+}
+
+int btConvexPointCloudShape::getNumEdges() const
+{
+	return 0;
+}
+
+void btConvexPointCloudShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const
+{
+	btAssert (0);
+}
+
+void btConvexPointCloudShape::getVertex(int i,btPoint3& vtx) const
+{
+	vtx = m_points[i]*m_localScaling;
+}
+
+int	btConvexPointCloudShape::getNumPlanes() const
+{
+	return 0;
+}
+
+void btConvexPointCloudShape::getPlane(btVector3& ,btPoint3& ,int ) const
+{
+
+	btAssert(0);
+}
+
+//not yet
+bool btConvexPointCloudShape::isInside(const btPoint3& ,btScalar ) const
+{
+	assert(0);
+	return false;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.h	(revision 1963)
@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H
+#define BT_CONVEX_POINT_CLOUD_SHAPE_H
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btAlignedObjectArray.h"
+
+///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
+ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape
+{
+	btVector3* m_points;
+	int m_numPoints;
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btConvexPointCloudShape(btVector3* points,int numPoints, bool computeAabb = true)
+	{
+		m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
+		m_points = points;
+		m_numPoints = numPoints;
+
+		if (computeAabb)
+			recalcLocalAabb();
+	}
+
+	void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
+	{
+		m_points = points;
+		m_numPoints = numPoints;
+
+		if (computeAabb)
+			recalcLocalAabb();
+	}
+
+	btPoint3* getPoints()
+	{
+		return m_points;
+	}
+
+	const btPoint3* getPoints() const
+	{
+		return m_points;
+	}
+
+	int getNumPoints() const 
+	{
+		return m_numPoints;
+	}
+
+#ifndef __SPU__
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+#endif
+
+
+	//debugging
+	virtual const char*	getName()const {return "ConvexPointCloud";}
+
+	virtual int	getNumVertices() const;
+	virtual int getNumEdges() const;
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
+	virtual void getVertex(int i,btPoint3& vtx) const;
+	virtual int	getNumPlanes() const;
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const;
+
+	///in case we receive negative scaling
+	virtual void	setLocalScaling(const btVector3& scaling);
+};
+
+
+#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp	(revision 1963)
@@ -0,0 +1,551 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexShape.h"
+#include "btConvexInternalShape.h"
+#include "btTriangleShape.h"
+#include "btSphereShape.h"
+#include "btCylinderShape.h"
+#include "btCapsuleShape.h"
+#include "btConvexHullShape.h"
+#include "btConvexPointCloudShape.h"
+
+static btVector3 convexHullSupport (const btVector3& localDir, const btVector3* points, int numPoints)
+{
+	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btScalar newDot,maxDot = btScalar(-1e30);
+
+	btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else {
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+
+	for (int i=0;i<numPoints;i++)
+	{
+		btPoint3 vtx = points[i];// * m_localScaling;
+
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+	return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
+}
+
+btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
+{
+	switch (m_shapeType)
+	{
+    case SPHERE_SHAPE_PROXYTYPE:
+	{
+		return btVector3(0,0,0);
+    }
+	break;
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
+		const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
+
+		return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
+			btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
+			btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
+	}
+	break;
+	case TRIANGLE_SHAPE_PROXYTYPE:
+	{
+		btTriangleShape* triangleShape = (btTriangleShape*)this;
+		btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
+		btVector3* vertices = &triangleShape->m_vertices1[0];
+		btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
+		btVector3 sup = vertices[dots.maxAxis()];
+		return btVector3(sup.getX(),sup.getY(),sup.getZ());
+	}
+	break;
+	case CYLINDER_SHAPE_PROXYTYPE:
+	{
+		btCylinderShape* cylShape = (btCylinderShape*)this;
+		//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
+
+		btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
+		btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
+		int cylinderUpAxis = cylShape->getUpAxis();
+		int XX(1),YY(0),ZZ(2);
+
+		switch (cylinderUpAxis)
+		{
+		case 0:
+		{
+			XX = 1;
+			YY = 0;
+			ZZ = 2;
+		}
+		break;
+		case 1:
+		{
+			XX = 0;
+			YY = 1;
+			ZZ = 2;	
+		}
+		break;
+		case 2:
+		{
+			XX = 0;
+			YY = 2;
+			ZZ = 1;
+			
+		}
+		break;
+		default:
+			btAssert(0);
+		break;
+		};
+
+		btScalar radius = halfExtents[XX];
+		btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+		btVector3 tmp;
+		btScalar d ;
+
+		btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+		if (s != btScalar(0.0))
+		{
+			d = radius / s;  
+			tmp[XX] = v[XX] * d;
+			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+			tmp[ZZ] = v[ZZ] * d;
+			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
+		} else {
+			tmp[XX] = radius;
+			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+			tmp[ZZ] = btScalar(0.0);
+			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
+		}
+	}
+	break;
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+		btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
+		btScalar halfHeight = capsuleShape->getHalfHeight();
+		int capsuleUpAxis = capsuleShape->getUpAxis();
+
+		btScalar radius = capsuleShape->getRadius();
+		btVector3 supVec(0,0,0);
+
+		btScalar maxDot(btScalar(-1e30));
+
+		btVector3 vec = vec0;
+		btScalar lenSqr = vec.length2();
+		if (lenSqr < btScalar(0.0001))
+		{
+			vec.setValue(1,0,0);
+		} else
+		{
+			btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+			vec *= rlen;
+		}
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = halfHeight;
+
+			//vtx = pos +vec*(radius);
+			vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
+			newDot = vec.dot(vtx);
+			
+
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = -halfHeight;
+
+			//vtx = pos +vec*(radius);
+			vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV();
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());	
+	}
+	break;
+	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+	{
+		btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
+		btVector3* points = convexPointCloudShape->getPoints ();
+		int numPoints = convexPointCloudShape->getNumPoints ();
+		return convexHullSupport (localDir, points, numPoints);
+	}
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
+		btPoint3* points = convexHullShape->getPoints ();
+		int numPoints = convexHullShape->getNumPoints ();
+		return convexHullSupport (localDir, points, numPoints);
+	}
+	break;
+    default:
+#ifndef __SPU__
+		return this->localGetSupportingVertexWithoutMargin (localDir);
+#else
+		btAssert (0);
+#endif
+	break;
+	}
+
+	// should never reach here
+	btAssert (0);
+	return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
+}
+
+btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const
+{
+	btVector3 localDirNorm = localDir;
+	if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+	}
+	localDirNorm.normalize ();
+	switch (m_shapeType)
+	{
+    case SPHERE_SHAPE_PROXYTYPE:
+	{
+		return btVector3(0,0,0) + getMarginNonVirtual() * localDirNorm;
+    }
+	break;
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
+		const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
+
+		return btVector3(localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(),
+						localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(),
+						localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()) + getMarginNonVirtual() * localDirNorm;
+	}
+	break;
+	case TRIANGLE_SHAPE_PROXYTYPE:
+	{
+		btTriangleShape* triangleShape = (btTriangleShape*)this;
+		btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
+		btVector3* vertices = &triangleShape->m_vertices1[0];
+		btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2]));
+		btVector3 sup = vertices[dots.maxAxis()];
+		return btVector3(sup.getX(),sup.getY(),sup.getZ()) + getMarginNonVirtual() * localDirNorm;
+	}
+	break;
+	case CYLINDER_SHAPE_PROXYTYPE:
+	{
+		btCylinderShape* cylShape = (btCylinderShape*)this;
+		//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
+
+		btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
+		btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
+		int cylinderUpAxis = cylShape->getUpAxis();
+		int XX(1),YY(0),ZZ(2);
+
+		switch (cylinderUpAxis)
+		{
+		case 0:
+		{
+			XX = 1;
+			YY = 0;
+			ZZ = 2;
+		}
+		break;
+		case 1:
+		{
+			XX = 0;
+			YY = 1;
+			ZZ = 2;	
+		}
+		break;
+		case 2:
+		{
+			XX = 0;
+			YY = 2;
+			ZZ = 1;
+			
+		}
+		break;
+		default:
+			btAssert(0);
+		break;
+		};
+
+		btScalar radius = halfExtents[XX];
+		btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+		btVector3 tmp;
+		btScalar d ;
+
+		btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+		if (s != btScalar(0.0))
+		{
+			d = radius / s;  
+			tmp[XX] = v[XX] * d;
+			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+			tmp[ZZ] = v[ZZ] * d;
+			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
+		} else {
+			tmp[XX] = radius;
+			tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+			tmp[ZZ] = btScalar(0.0);
+			return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()) + getMarginNonVirtual() * localDirNorm;
+		}
+	}
+	break;
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
+
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+		btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
+		btScalar halfHeight = capsuleShape->getHalfHeight();
+		int capsuleUpAxis = capsuleShape->getUpAxis();
+
+		btScalar radius = capsuleShape->getRadius();
+		btVector3 supVec(0,0,0);
+
+		btScalar maxDot(btScalar(-1e30));
+
+		btVector3 vec = vec0;
+		btScalar lenSqr = vec.length2();
+		if (lenSqr < btScalar(0.0001))
+		{
+			vec.setValue(1,0,0);
+		} else
+		{
+			btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+			vec *= rlen;
+		}
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		{
+			btVector3 pos(0,0,0);
+			pos[capsuleUpAxis] = -halfHeight;
+
+			vtx = pos +vec*(radius);
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supVec = vtx;
+			}
+		}
+		return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()) + getMarginNonVirtual() * localDirNorm;
+	}
+	break;
+	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+	{
+		btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
+		btVector3* points = convexPointCloudShape->getPoints ();
+		int numPoints = convexPointCloudShape->getNumPoints ();
+		return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
+	}
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
+		btPoint3* points = convexHullShape->getPoints ();
+		int numPoints = convexHullShape->getNumPoints ();
+		return convexHullSupport (localDir, points, numPoints) + getMarginNonVirtual() * localDirNorm;
+	}
+	break;
+    default:
+#ifndef __SPU__
+		return this->localGetSupportingVertex (localDir);
+#else
+		btAssert (0);
+#endif
+	break;
+	}
+
+	// should never reach here
+	btAssert (0);
+	return btPoint3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
+}
+
+/* TODO: This should be bumped up to btCollisionShape () */
+btScalar btConvexShape::getMarginNonVirtual () const
+{
+	switch (m_shapeType)
+	{
+    case SPHERE_SHAPE_PROXYTYPE:
+	{
+		btSphereShape* sphereShape = (btSphereShape*)this;
+		return sphereShape->getRadius ();
+	}
+	break;
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
+		return convexShape->getMarginNV ();
+	}
+	break;
+	case TRIANGLE_SHAPE_PROXYTYPE:
+	{
+		btTriangleShape* triangleShape = (btTriangleShape*)this;
+		return triangleShape->getMarginNV ();
+	}
+	break;
+	case CYLINDER_SHAPE_PROXYTYPE:
+	{
+		btCylinderShape* cylShape = (btCylinderShape*)this;
+		return cylShape->getMarginNV();
+	}
+	break;
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+		return capsuleShape->getMarginNV();
+	}
+	break;
+	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+	/* fall through */
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
+		return convexHullShape->getMarginNV();
+	}
+	break;
+    default:
+#ifndef __SPU__
+		return this->getMargin ();
+#else
+		btAssert (0);
+#endif
+	break;
+	}
+
+	// should never reach here
+	btAssert (0);
+	return btScalar(0.0f);
+}
+
+void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
+{
+	switch (m_shapeType)
+	{
+    case SPHERE_SHAPE_PROXYTYPE:
+	{
+		btSphereShape* sphereShape = (btSphereShape*)this;
+		float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
+		float margin = radius + sphereShape->getMarginNonVirtual();
+		const btVector3& center = t.getOrigin();
+		btVector3 extent(margin,margin,margin);
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+    }
+	break;
+	case CYLINDER_SHAPE_PROXYTYPE:
+	/* fall through */
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		btConvexInternalShape* convexShape = (btConvexInternalShape*)this;
+		float margin=convexShape->getMarginNonVirtual();
+		btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
+		halfExtents += btVector3(margin,margin,margin);
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
+		
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+		break;
+	}
+	break;
+	case TRIANGLE_SHAPE_PROXYTYPE:
+	{
+		btTriangleShape* triangleShape = (btTriangleShape*)this;
+		btScalar margin = triangleShape->getMarginNonVirtual();
+		for (int i=0;i<3;i++)
+		{
+			btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+			vec[i] = btScalar(1.);
+
+			btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
+
+			btVector3 tmp = t(sv);
+			aabbMax[i] = tmp[i]+margin;
+			vec[i] = btScalar(-1.);
+			tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
+			aabbMin[i] = tmp[i]-margin;
+		}	
+	}
+	break;
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
+		btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius());
+		int m_upAxis = capsuleShape->getUpAxis();
+		halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight();
+		halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual());
+		btMatrix3x3 abs_b = t.getBasis().absolute();  
+		btPoint3 center = t.getOrigin();
+		btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));		  	
+		aabbMin = center - extent;
+		aabbMax = center + extent;
+	}
+	break;
+	case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this;
+		btScalar margin = convexHullShape->getMarginNonVirtual();
+		convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
+	}
+	break;
+    default:
+#ifndef __SPU__
+		this->getAabb (t, aabbMin, aabbMax);
+#else
+		btAssert (0);
+#endif
+	break;
+	}
+
+	// should never reach here
+	btAssert (0);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexShape.h	(revision 1963)
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONVEX_SHAPE_INTERFACE1
+#define CONVEX_SHAPE_INTERFACE1
+
+#include "btCollisionShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+//todo: get rid of this btConvexCastResult thing!
+struct btConvexCastResult;
+#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10
+
+/// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc.
+/// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector.
+ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape
+{
+
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btConvexShape ()
+	{
+	}
+
+	virtual ~btConvexShape()
+	{
+
+	}
+
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const =0;
+#ifndef __SPU__
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0;
+	
+	//notice that the vectors should be unit length
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0;
+#endif //#ifndef __SPU__
+
+	btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const;
+	btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const;
+	btScalar getMarginNonVirtual () const;
+	void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0;
+
+	virtual void	setLocalScaling(const btVector3& scaling) =0;
+	virtual const btVector3& getLocalScaling() const =0;
+
+	virtual void	setMargin(btScalar margin)=0;
+
+	virtual btScalar	getMargin() const=0;
+
+	virtual int		getNumPreferredPenetrationDirections() const=0;
+	
+	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0;
+
+};
+
+
+
+#endif //CONVEX_SHAPE_INTERFACE1
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp	(revision 1963)
@@ -0,0 +1,317 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+
+
+btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
+: btPolyhedralConvexShape(), m_stridingMesh(meshInterface)
+{
+	m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
+	if ( calcAabb )
+		recalcLocalAabb();
+}
+
+
+
+
+///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
+///but then we are duplicating
+class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
+{
+
+	btVector3 m_supportVertexLocal;
+public:
+
+	btScalar m_maxDot;
+	btVector3 m_supportVecLocal;
+
+	LocalSupportVertexCallback(const btVector3& supportVecLocal)
+		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
+		m_maxDot(btScalar(-1e30)),
+                m_supportVecLocal(supportVecLocal)
+	{
+	}
+
+	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	{
+		(void)triangleIndex;
+		(void)partId;
+
+		for (int i=0;i<3;i++)
+		{
+			btScalar dot = m_supportVecLocal.dot(triangle[i]);
+			if (dot > m_maxDot)
+			{
+				m_maxDot = dot;
+				m_supportVertexLocal = triangle[i];
+			}
+		}
+	}
+	
+	btVector3	GetSupportVertexLocal()
+	{
+		return m_supportVertexLocal;
+	}
+
+};
+
+
+
+
+
+btVector3	btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	LocalSupportVertexCallback	supportCallback(vec);
+	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+	supVec = supportCallback.GetSupportVertexLocal();
+
+	return supVec;
+}
+
+void	btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	//use 'w' component of supportVerticesOut?
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			supportVerticesOut[i][3] = btScalar(-1e30);
+		}
+	}
+	
+	//todo: could do the batch inside the callback!
+
+
+	for (int j=0;j<numVectors;j++)
+	{
+		const btVector3& vec = vectors[j];
+		LocalSupportVertexCallback	supportCallback(vec);
+		btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+		m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+		supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
+	}
+	
+}
+	
+
+
+btVector3	btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const
+{
+	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+	if ( getMargin()!=btScalar(0.) )
+	{
+		btVector3 vecnorm = vec;
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		{
+			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+		} 
+		vecnorm.normalize();
+		supVertex+= getMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+
+
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
+int	btConvexTriangleMeshShape::getNumVertices() const
+{
+	//cache this?
+	return 0;
+	
+}
+
+int btConvexTriangleMeshShape::getNumEdges() const
+{
+	return 0;
+}
+
+void btConvexTriangleMeshShape::getEdge(int ,btPoint3& ,btPoint3& ) const
+{
+	btAssert(0);	
+}
+
+void btConvexTriangleMeshShape::getVertex(int ,btPoint3& ) const
+{
+	btAssert(0);
+}
+
+int	btConvexTriangleMeshShape::getNumPlanes() const
+{
+	return 0;
+}
+
+void btConvexTriangleMeshShape::getPlane(btVector3& ,btPoint3& ,int  ) const
+{
+	btAssert(0);
+}
+
+//not yet
+bool btConvexTriangleMeshShape::isInside(const btPoint3& ,btScalar ) const
+{
+	btAssert(0);
+	return false;
+}
+
+
+
+void	btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+	m_stridingMesh->setScaling(scaling);
+	
+	recalcLocalAabb();
+	
+}
+
+
+const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
+{
+	return m_stridingMesh->getScaling();
+}
+
+void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
+{
+   class CenterCallback: public btInternalTriangleIndexCallback
+   {
+      bool first;
+      btVector3 ref;
+      btVector3 sum;
+      btScalar volume;
+
+   public:
+
+      CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         if (first)
+         {
+            ref = triangle[0];
+            first = false;
+         }
+         else
+         {
+            btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+            sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+            volume += vol;
+         }
+      }
+      
+      btVector3 getCenter()
+      {
+         return (volume > 0) ? sum / volume : ref;
+      }
+
+      btScalar getVolume()
+      {
+         return volume * btScalar(1. / 6);
+      }
+
+   };
+
+   class InertiaCallback: public btInternalTriangleIndexCallback
+   {
+      btMatrix3x3 sum;
+      btVector3 center;
+
+   public:
+
+      InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         btMatrix3x3 i;
+         btVector3 a = triangle[0] - center;
+         btVector3 b = triangle[1] - center;
+         btVector3 c = triangle[2] - center;
+         btVector3 abc = a + b + c;
+         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+         for (int j = 0; j < 3; j++)
+         {
+            for (int k = 0; k <= j; k++)
+            {
+               i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
+                 + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
+                  + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+            }
+         }
+         btScalar i00 = -i[0][0];
+         btScalar i11 = -i[1][1];
+         btScalar i22 = -i[2][2];
+         i[0][0] = i11 + i22; 
+         i[1][1] = i22 + i00; 
+         i[2][2] = i00 + i11;
+         sum[0] += i[0];
+         sum[1] += i[1];
+         sum[2] += i[2];
+      }
+      
+      btMatrix3x3& getInertia()
+      {
+         return sum;
+      }
+
+   };
+
+   CenterCallback centerCallback;
+   btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+   m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
+   btVector3 center = centerCallback.getCenter();
+   principal.setOrigin(center);
+   volume = centerCallback.getVolume();
+
+   InertiaCallback inertiaCallback(center);
+   m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+   btMatrix3x3& i = inertiaCallback.getInertia();
+   i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+   inertia.setValue(i[0][0], i[1][1], i[2][2]);
+   inertia /= volume;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h	(revision 1963)
@@ -0,0 +1,61 @@
+#ifndef CONVEX_TRIANGLEMESH_SHAPE_H
+#define CONVEX_TRIANGLEMESH_SHAPE_H
+
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+
+/// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape.
+/// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead.
+class btConvexTriangleMeshShape : public btPolyhedralConvexShape
+{
+
+	class btStridingMeshInterface*	m_stridingMesh;
+
+public:
+	btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true);
+
+	class btStridingMeshInterface*	getMeshInterface()
+	{
+		return m_stridingMesh;
+	}
+	const class btStridingMeshInterface* getMeshInterface() const
+	{
+		return m_stridingMesh;
+	}
+	
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	
+	//debugging
+	virtual const char*	getName()const {return "ConvexTrimesh";}
+	
+	virtual int	getNumVertices() const;
+	virtual int getNumEdges() const;
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
+	virtual void getVertex(int i,btPoint3& vtx) const;
+	virtual int	getNumPlanes() const;
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const;
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const;
+
+	
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+
+	///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+	///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
+	///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
+	///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+	///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
+	void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const;
+
+};
+
+
+
+#endif //CONVEX_TRIANGLEMESH_SHAPE_H
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.cpp	(revision 1963)
@@ -0,0 +1,207 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btCylinderShape.h"
+#include "LinearMath/btPoint3.h"
+
+btCylinderShape::btCylinderShape (const btVector3& halfExtents)
+:btBoxShape(halfExtents),
+m_upAxis(1)
+{
+	m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
+	recalcLocalAabb();
+}
+
+
+btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
+:btCylinderShape(halfExtents)
+{
+	m_upAxis = 0;
+	recalcLocalAabb();
+}
+
+
+btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
+:btCylinderShape(halfExtents)
+{
+	m_upAxis = 2;
+	recalcLocalAabb();
+}
+
+void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	//skip the box 'getAabb'
+	btPolyhedralConvexShape::getAabb(t,aabbMin,aabbMax);
+}
+
+
+SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) 
+{
+const int cylinderUpAxis = 0;
+const int XX = 1;
+const int YY = 0;
+const int ZZ = 2;
+
+	//mapping depends on how cylinder local orientation is
+	// extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+	btScalar radius = halfExtents[XX];
+	btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+
+    btVector3 tmp;
+	btScalar d ;
+
+    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != btScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = btScalar(0.0);
+		return tmp;
+    }
+
+
+}
+
+
+
+
+
+
+inline  btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) 
+{
+
+const int cylinderUpAxis = 1;
+const int XX = 0;
+const int YY = 1;
+const int ZZ = 2;
+
+
+	btScalar radius = halfExtents[XX];
+	btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+
+    btVector3 tmp;
+	btScalar d ;
+
+    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != btScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = btScalar(0.0);
+		return tmp;
+    }
+
+}
+
+inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) 
+{
+const int cylinderUpAxis = 2;
+const int XX = 0;
+const int YY = 2;
+const int ZZ = 1;
+
+	//mapping depends on how cylinder local orientation is
+	// extents of the cylinder is: X,Y is for radius, and Z for height
+
+
+	btScalar radius = halfExtents[XX];
+	btScalar halfHeight = halfExtents[cylinderUpAxis];
+
+
+    btVector3 tmp;
+	btScalar d ;
+
+    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
+    if (s != btScalar(0.0))
+	{
+        d = radius / s;  
+		tmp[XX] = v[XX] * d;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = v[ZZ] * d;
+		return tmp;
+	}
+    else
+	{
+	    tmp[XX] = radius;
+		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
+		tmp[ZZ] = btScalar(0.0);
+		return tmp;
+    }
+
+
+}
+
+btVector3	btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
+}
+
+
+btVector3	btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
+}
+btVector3	btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
+}
+
+void	btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
+	}
+}
+
+void	btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
+	}
+}
+
+
+
+
+void	btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
+	}
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btCylinderShape.h	(revision 1963)
@@ -0,0 +1,134 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CYLINDER_MINKOWSKI_H
+#define CYLINDER_MINKOWSKI_H
+
+#include "btBoxShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "LinearMath/btVector3.h"
+
+/// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis.
+class btCylinderShape : public btBoxShape
+
+{
+
+protected:
+
+	int	m_upAxis;
+
+public:
+	btCylinderShape (const btVector3& halfExtents);
+	
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	{
+
+		btVector3 supVertex;
+		supVertex = localGetSupportingVertexWithoutMargin(vec);
+		
+		if ( getMargin()!=btScalar(0.) )
+		{
+			btVector3 vecnorm = vec;
+			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+			{
+				vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+			} 
+			vecnorm.normalize();
+			supVertex+= getMargin() * vecnorm;
+		}
+		return supVertex;
+	}
+
+
+	//use box inertia
+	//	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+	int	getUpAxis() const
+	{
+		return m_upAxis;
+	}
+
+	virtual btScalar getRadius() const
+	{
+		return getHalfExtentsWithMargin().getX();
+	}
+
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "CylinderY";
+	}
+
+
+
+};
+
+class btCylinderShapeX : public btCylinderShape
+{
+public:
+	btCylinderShapeX (const btVector3& halfExtents);
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	
+		//debugging
+	virtual const char*	getName()const
+	{
+		return "CylinderX";
+	}
+
+	virtual btScalar getRadius() const
+	{
+		return getHalfExtentsWithMargin().getY();
+	}
+
+};
+
+class btCylinderShapeZ : public btCylinderShape
+{
+public:
+	btCylinderShapeZ (const btVector3& halfExtents);
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+	virtual int	getUpAxis() const
+	{
+		return 2;
+	}
+		//debugging
+	virtual const char*	getName()const
+	{
+		return "CylinderZ";
+	}
+
+	virtual btScalar getRadius() const
+	{
+		return getHalfExtentsWithMargin().getX();
+	}
+
+};
+
+
+#endif //CYLINDER_MINKOWSKI_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.cpp	(revision 1963)
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btEmptyShape.h"
+
+
+#include "btCollisionShape.h"
+
+
+btEmptyShape::btEmptyShape() : btConcaveShape ()
+{
+	m_shapeType = EMPTY_SHAPE_PROXYTYPE;
+}
+
+
+btEmptyShape::~btEmptyShape()
+{
+}
+
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btVector3 margin(getMargin(),getMargin(),getMargin());
+
+	aabbMin = t.getOrigin() - margin;
+
+	aabbMax = t.getOrigin() + margin;
+
+}
+
+void	btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const
+{
+	btAssert(0);
+}
+
+	
+	
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btEmptyShape.h	(revision 1963)
@@ -0,0 +1,67 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef EMPTY_SHAPE_H
+#define EMPTY_SHAPE_H
+
+#include "btConcaveShape.h"
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "btCollisionMargin.h"
+
+
+
+
+/// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class.
+/// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed.
+class btEmptyShape	: public btConcaveShape
+{
+public:
+	btEmptyShape();
+
+	virtual ~btEmptyShape();
+
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		m_localScaling = scaling;
+	}
+	virtual const btVector3& getLocalScaling() const 
+	{
+		return m_localScaling;
+	}
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+	
+	virtual const char*	getName()const
+	{
+		return "Empty";
+	}
+
+
+protected:
+	btVector3	m_localScaling;
+
+};
+
+
+
+#endif //EMPTY_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp	(revision 1963)
@@ -0,0 +1,336 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btHeightfieldTerrainShape.h"
+
+#include "LinearMath/btTransformUtil.h"
+
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
+: btConcaveShape (), m_heightStickWidth(heightStickWidth),
+m_heightStickLength(heightStickLength),
+m_maxHeight(maxHeight),
+m_width((btScalar)heightStickWidth-1),
+m_length((btScalar)heightStickLength-1),
+m_heightfieldDataUnknown(heightfieldData),
+m_useFloatData(useFloatData),
+m_flipQuadEdges(flipQuadEdges),
+m_useDiamondSubdivision(false),
+m_upAxis(upAxis),
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+{
+	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
+
+	btScalar	quantizationMargin = 1.f;
+
+	//enlarge the AABB to avoid division by zero when initializing the quantization values
+	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+
+	btVector3	halfExtents(0,0,0);
+
+	switch (m_upAxis)
+	{
+	case 0:
+		{
+			halfExtents.setValue(
+				btScalar(m_maxHeight),
+				btScalar(m_width), //?? don't know if this should change
+				btScalar(m_length));
+			break;
+		}
+	case 1:
+		{
+			halfExtents.setValue(
+				btScalar(m_width),
+				btScalar(m_maxHeight),
+				btScalar(m_length));
+			break;
+		};
+	case 2:
+		{
+			halfExtents.setValue(
+				btScalar(m_width),
+				btScalar(m_length),
+				btScalar(m_maxHeight)
+			);
+			break;
+		}
+	default:
+		{
+			//need to get valid m_upAxis
+			btAssert(0);
+		}
+	}
+
+	halfExtents*= btScalar(0.5);
+	
+	m_localAabbMin = -halfExtents - clampValue;
+	m_localAabbMax = halfExtents + clampValue;
+	btVector3 aabbSize = m_localAabbMax - m_localAabbMin;
+
+}
+
+
+btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
+{
+}
+
+
+
+void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
+	halfExtents += btVector3(getMargin(),getMargin(),getMargin());
+
+	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btPoint3 center = t.getOrigin();
+	btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
+		   abs_b[1].dot(halfExtents),
+		  abs_b[2].dot(halfExtents));
+	
+
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+
+
+}
+
+btScalar	btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const
+{
+	btScalar val = 0.f;
+	if (m_useFloatData)
+	{
+		val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
+	} else
+	{
+		//assume unsigned short int
+		unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
+		val = heightFieldValue* (m_maxHeight/btScalar(65535));
+	}
+	return val;
+}
+
+
+
+
+
+void	btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
+{
+
+	btAssert(x>=0);
+	btAssert(y>=0);
+	btAssert(x<m_heightStickWidth);
+	btAssert(y<m_heightStickLength);
+
+
+	btScalar	height = getHeightFieldValue(x,y);
+
+	switch (m_upAxis)
+	{
+	case 0:
+		{
+		vertex.setValue(
+			height,
+			(-m_width/btScalar(2.0)) + x,
+			(-m_length/btScalar(2.0) ) + y
+			);
+			break;
+		}
+	case 1:
+		{
+			vertex.setValue(
+			(-m_width/btScalar(2.0)) + x,
+			height,
+			(-m_length/btScalar(2.0)) + y
+			);
+			break;
+		};
+	case 2:
+		{
+			vertex.setValue(
+			(-m_width/btScalar(2.0)) + x,
+			(-m_length/btScalar(2.0)) + y,
+			height
+			);
+			break;
+		}
+	default:
+		{
+			//need to get valid m_upAxis
+			btAssert(0);
+		}
+	}
+
+	vertex*=m_localScaling;
+	
+}
+
+
+void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
+{
+	btVector3 clampedPoint(point);
+	clampedPoint.setMax(m_localAabbMin);
+	clampedPoint.setMin(m_localAabbMax);
+
+	btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization;
+
+	//TODO: optimization: check out how to removed this btFabs
+		
+	out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) );
+	out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) );
+	out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) );
+	
+}
+
+
+void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	(void)callback;
+	(void)aabbMax;
+	(void)aabbMin;
+
+	//quantize the aabbMin and aabbMax, and adjust the start/end ranges
+
+	int	quantizedAabbMin[3];
+	int	quantizedAabbMax[3];
+
+	btVector3	localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+	btVector3	localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+	
+	quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
+	quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
+	
+	
+
+	int startX=0;
+	int endX=m_heightStickWidth-1;
+	int startJ=0;
+	int endJ=m_heightStickLength-1;
+
+	switch (m_upAxis)
+	{
+	case 0:
+		{
+			quantizedAabbMin[1]+=m_heightStickWidth/2-1;
+			quantizedAabbMax[1]+=m_heightStickWidth/2+1;
+			quantizedAabbMin[2]+=m_heightStickLength/2-1;
+			quantizedAabbMax[2]+=m_heightStickLength/2+1;
+
+			if (quantizedAabbMin[1]>startX)
+				startX = quantizedAabbMin[1];
+			if (quantizedAabbMax[1]<endX)
+				endX = quantizedAabbMax[1];
+			if (quantizedAabbMin[2]>startJ)
+				startJ = quantizedAabbMin[2];
+			if (quantizedAabbMax[2]<endJ)
+				endJ = quantizedAabbMax[2];
+			break;
+		}
+	case 1:
+		{
+			quantizedAabbMin[0]+=m_heightStickWidth/2-1;
+			quantizedAabbMax[0]+=m_heightStickWidth/2+1;
+			quantizedAabbMin[2]+=m_heightStickLength/2-1;
+			quantizedAabbMax[2]+=m_heightStickLength/2+1;
+
+			if (quantizedAabbMin[0]>startX)
+				startX = quantizedAabbMin[0];
+			if (quantizedAabbMax[0]<endX)
+				endX = quantizedAabbMax[0];
+			if (quantizedAabbMin[2]>startJ)
+				startJ = quantizedAabbMin[2];
+			if (quantizedAabbMax[2]<endJ)
+				endJ = quantizedAabbMax[2];
+			break;
+		};
+	case 2:
+		{
+			quantizedAabbMin[0]+=m_heightStickWidth/2-1;
+			quantizedAabbMax[0]+=m_heightStickWidth/2+1;
+			quantizedAabbMin[1]+=m_heightStickLength/2-1;
+			quantizedAabbMax[1]+=m_heightStickLength/2+1;
+
+			if (quantizedAabbMin[0]>startX)
+				startX = quantizedAabbMin[0];
+			if (quantizedAabbMax[0]<endX)
+				endX = quantizedAabbMax[0];
+			if (quantizedAabbMin[1]>startJ)
+				startJ = quantizedAabbMin[1];
+			if (quantizedAabbMax[1]<endJ)
+				endJ = quantizedAabbMax[1];
+			break;
+		}
+	default:
+		{
+			//need to get valid m_upAxis
+			btAssert(0);
+		}
+	}
+
+	
+  
+
+	for(int j=startJ; j<endJ; j++)
+	{
+		for(int x=startX; x<endX; x++)
+		{
+			btVector3 vertices[3];
+			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1)))
+			{
+        //first triangle
+        getVertex(x,j,vertices[0]);
+        getVertex(x+1,j,vertices[1]);
+        getVertex(x+1,j+1,vertices[2]);
+        callback->processTriangle(vertices,x,j);
+        //second triangle
+        getVertex(x,j,vertices[0]);
+        getVertex(x+1,j+1,vertices[1]);
+        getVertex(x,j+1,vertices[2]);
+        callback->processTriangle(vertices,x,j);				
+			} else
+			{
+        //first triangle
+        getVertex(x,j,vertices[0]);
+        getVertex(x,j+1,vertices[1]);
+        getVertex(x+1,j,vertices[2]);
+        callback->processTriangle(vertices,x,j);
+        //second triangle
+        getVertex(x+1,j,vertices[0]);
+        getVertex(x,j+1,vertices[1]);
+        getVertex(x+1,j+1,vertices[2]);
+        callback->processTriangle(vertices,x,j);
+			}
+		}
+	}
+
+	
+
+}
+
+void	btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
+{
+	//moving concave objects not supported
+	
+	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void	btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling;
+}
+const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
+{
+	return m_localScaling;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h	(revision 1963)
@@ -0,0 +1,87 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H
+#define HEIGHTFIELD_TERRAIN_SHAPE_H
+
+#include "btConcaveShape.h"
+
+///The btHeightfieldTerrainShape simulates a 2D heightfield terrain collision shape. You can also use the more general btBvhTriangleMeshShape instead.
+///An example implementation of btHeightfieldTerrainShape is provided in Demos/VehicleDemo/VehicleDemo.cpp
+class btHeightfieldTerrainShape : public btConcaveShape
+{
+protected:
+	btVector3	m_localAabbMin;
+	btVector3	m_localAabbMax;
+	
+	///terrain data
+	int	m_heightStickWidth;
+	int m_heightStickLength;
+	btScalar	m_maxHeight;
+	btScalar m_width;
+	btScalar m_length;
+	union
+	{
+		unsigned char*	m_heightfieldDataUnsignedChar;
+		btScalar*			m_heightfieldDataFloat;
+		void*			m_heightfieldDataUnknown;
+	};
+	
+	bool	m_useFloatData;
+	bool	m_flipQuadEdges;
+  bool  m_useDiamondSubdivision;
+
+	int	m_upAxis;
+	
+	btVector3	m_localScaling;
+
+	virtual btScalar	getHeightFieldValue(int x,int y) const;
+	void		quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
+	void		getVertex(int x,int y,btVector3& vertex) const;
+
+	inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const  int* aabbMin2,const  int* aabbMax2) const
+	{
+		bool overlap = true;
+		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+		return overlap;
+	}
+
+public:
+	btHeightfieldTerrainShape(int heightStickWidth,int heightStickHeight,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
+
+	virtual ~btHeightfieldTerrainShape();
+
+
+	void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;}
+
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	
+	virtual const btVector3& getLocalScaling() const;
+	
+	//debugging
+	virtual const char*	getName()const {return "HEIGHTFIELD";}
+
+};
+
+#endif //HEIGHTFIELD_TERRAIN_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMaterial.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMaterial.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMaterial.h	(revision 1963)
@@ -0,0 +1,34 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// This file was created by Alex Silverman
+
+#ifndef MATERIAL_H
+#define MATERIAL_H
+
+// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
+class btMaterial
+{
+    // public members so that materials can change due to world events
+public:
+    btScalar m_friction;
+    btScalar m_restitution;
+    int pad[2];
+
+    btMaterial(){}
+    btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
+};
+
+#endif // MATERIAL_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp	(revision 1963)
@@ -0,0 +1,59 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMinkowskiSumShape.h"
+
+
+btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB)
+: btConvexInternalShape (),
+m_shapeA(shapeA),
+m_shapeB(shapeB)
+{
+	m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE;
+	m_transA.setIdentity();
+	m_transB.setIdentity();
+}
+
+btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis()));
+	btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
+	return  supVertexA - supVertexB;
+}
+
+void	btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	//todo: could make recursive use of batching. probably this shape is not used frequently.
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
+	}
+
+}
+
+
+
+btScalar	btMinkowskiSumShape::getMargin() const
+{
+	return m_shapeA->getMargin() + m_shapeB->getMargin();
+}
+
+
+void	btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	(void)mass;
+	btAssert(0);
+	inertia.setValue(0,0,0);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.h	(revision 1963)
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_SUM_SHAPE_H
+#define MINKOWSKI_SUM_SHAPE_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+/// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes.
+class btMinkowskiSumShape : public btConvexInternalShape
+{
+
+	btTransform	m_transA;
+	btTransform	m_transB;
+	const btConvexShape*	m_shapeA;
+	const btConvexShape*	m_shapeB;
+
+public:
+
+	btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB);
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	void	setTransformA(const btTransform&	transA) { m_transA = transA;}
+	void	setTransformB(const btTransform&	transB) { m_transB = transB;}
+
+	const btTransform& getTransformA()const  { return m_transA;}
+	const btTransform& GetTransformB()const  { return m_transB;}
+
+
+	virtual btScalar	getMargin() const;
+
+	const btConvexShape*	getShapeA() const { return m_shapeA;}
+	const btConvexShape*	getShapeB() const { return m_shapeB;}
+
+	virtual const char*	getName()const 
+	{
+		return "MinkowskiSum";
+	}
+};
+
+#endif //MINKOWSKI_SUM_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.cpp	(revision 1963)
@@ -0,0 +1,149 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btQuaternion.h"
+
+btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres)
+:btConvexInternalShape (), m_inertiaHalfExtents(inertiaHalfExtents)
+{
+	m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
+	btScalar startMargin = btScalar(1e30);
+
+	m_numSpheres = numSpheres;
+	for (int i=0;i<m_numSpheres;i++)
+	{
+		m_localPositions[i] = positions[i];
+		m_radi[i] = radi[i];
+		if (radi[i] < startMargin)
+			startMargin = radi[i];
+	}
+	setMargin(startMargin);
+
+}
+
+
+
+ 
+ btVector3	btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+	int i;
+	btVector3 supVec(0,0,0);
+
+	btScalar maxDot(btScalar(-1e30));
+
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	btVector3 vtx;
+	btScalar newDot;
+
+	const btVector3* pos = &m_localPositions[0];
+	const btScalar* rad = &m_radi[0];
+
+	for (i=0;i<m_numSpheres;i++)
+	{
+		vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+		pos++;
+		rad++;
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+
+	return supVec;
+
+}
+
+ void	btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+
+	for (int j=0;j<numVectors;j++)
+	{
+		btScalar maxDot(btScalar(-1e30));
+
+		const btVector3& vec = vectors[j];
+
+		btVector3 vtx;
+		btScalar newDot;
+
+		const btVector3* pos = &m_localPositions[0];
+		const btScalar* rad = &m_radi[0];
+
+		for (int i=0;i<m_numSpheres;i++)
+		{
+			vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+			pos++;
+			rad++;
+			newDot = vec.dot(vtx);
+			if (newDot > maxDot)
+			{
+				maxDot = newDot;
+				supportVerticesOut[j] = vtx;
+			}
+		}
+	}
+}
+
+
+
+
+
+
+
+
+void	btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	//as an approximation, take the inertia of the box that bounds the spheres
+
+	btTransform ident;
+	ident.setIdentity();
+//	btVector3 aabbMin,aabbMax;
+
+//	getAabb(ident,aabbMin,aabbMax);
+
+	btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* btScalar(0.5);
+
+	btScalar margin = CONVEX_DISTANCE_MARGIN;
+
+	btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
+	btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
+	btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(.08333333);
+
+	inertia[0] = scaledmass * (y2+z2);
+	inertia[1] = scaledmass * (x2+z2);
+	inertia[2] = scaledmass * (x2+y2);
+
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultiSphereShape.h	(revision 1963)
@@ -0,0 +1,74 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MULTI_SPHERE_MINKOWSKI_H
+#define MULTI_SPHERE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+#define MAX_NUM_SPHERES 5
+
+///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes.
+///It is possible to animate the spheres for deformation.
+class btMultiSphereShape : public btConvexInternalShape
+
+{
+	
+	btVector3 m_localPositions[MAX_NUM_SPHERES];
+	btScalar  m_radi[MAX_NUM_SPHERES];
+	btVector3	m_inertiaHalfExtents;
+
+	int m_numSpheres;
+	
+
+
+
+public:
+	btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres);
+
+	///CollisionShape Interface
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	/// btConvexShape Interface
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+	
+	int	getSphereCount() const
+	{
+		return m_numSpheres;
+	}
+
+	const btVector3&	getSpherePosition(int index) const
+	{
+		return m_localPositions[index];
+	}
+
+	btScalar	getSphereRadius(int index) const
+	{
+		return m_radi[index];
+	}
+
+
+	virtual const char*	getName()const 
+	{
+		return "MultiSphere";
+	}
+
+};
+
+
+#endif //MULTI_SPHERE_MINKOWSKI_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp	(revision 1963)
@@ -0,0 +1,45 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// This file was created by Alex Silverman
+
+#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
+//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
+
+
+///Obtains the material for a specific triangle
+const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
+{
+    const unsigned char * materialBase = 0;
+    int numMaterials;
+    PHY_ScalarType materialType;
+    int materialStride;
+    const unsigned char * triangleMaterialBase = 0;
+    int numTriangles;
+    int triangleMaterialStride;
+    PHY_ScalarType triangleType;
+
+    ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
+        &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
+
+    // return the pointer to the place with the friction for the triangle
+    // TODO: This depends on whether it's a moving mesh or not
+    // BUG IN GIMPACT
+    //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
+    int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
+    btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
+    return (matVal);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h	(revision 1963)
@@ -0,0 +1,124 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// This file was created by Alex Silverman
+
+#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
+#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
+
+#include "btBvhTriangleMeshShape.h"
+#include "btMaterial.h"
+
+///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
+ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
+{
+    btAlignedObjectArray <btMaterial*> m_materialList;
+    int ** m_triangleMaterials;
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {}
+    btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
+        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
+        {
+            btVector3 m_triangle[3];
+            const unsigned char *vertexbase;
+            int numverts;
+            PHY_ScalarType type;
+            int stride;
+            const unsigned char *indexbase;
+            int indexstride;
+            int numfaces;
+            PHY_ScalarType indicestype;
+
+            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
+            {
+                m_meshInterface->getLockedReadOnlyVertexIndexBase(
+                    &vertexbase,
+                    numverts,
+                    type,
+                    stride,
+                    &indexbase,
+                    indexstride,
+                    numfaces,
+                    indicestype,
+                    i);
+                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
+            }
+        }
+
+	///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
+	btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
+        btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
+        {
+            btVector3 m_triangle[3];
+            const unsigned char *vertexbase;
+            int numverts;
+            PHY_ScalarType type;
+            int stride;
+            const unsigned char *indexbase;
+            int indexstride;
+            int numfaces;
+            PHY_ScalarType indicestype;
+
+            //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
+
+            for(int i = 0; i < meshInterface->getNumSubParts(); i++)
+            {
+                m_meshInterface->getLockedReadOnlyVertexIndexBase(
+                    &vertexbase,
+                    numverts,
+                    type,
+                    stride,
+                    &indexbase,
+                    indexstride,
+                    numfaces,
+                    indicestype,
+                    i);
+                //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
+            }
+        }
+	
+    virtual ~btMultimaterialTriangleMeshShape()
+    {
+/*
+        for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
+        {
+            btAlignedFree(m_materialValues[i]);
+            m_materialLookup[i] = NULL;
+        }
+        btAlignedFree(m_materialValues);
+        m_materialLookup = NULL;
+*/
+    }
+	virtual int	getShapeType() const
+	{
+		return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+	}
+	
+	//debugging
+	virtual const char*	getName()const {return "MULTIMATERIALTRIANGLEMESH";}
+
+    ///Obtains the material for a specific triangle
+    const btMaterial * getMaterialProperties(int partID, int triIndex);
+
+}
+;
+
+#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp	(revision 1963)
@@ -0,0 +1,390 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btOptimizedBvh.h"
+#include "btStridingMeshInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+btOptimizedBvh::btOptimizedBvh()
+{ 
+}
+
+btOptimizedBvh::~btOptimizedBvh()
+{
+}
+
+
+void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
+{
+	m_useQuantization = useQuantizedAabbCompression;
+
+
+	// NodeArray	triangleNodes;
+
+	struct	NodeTriangleCallback : public btInternalTriangleIndexCallback
+	{
+
+		NodeArray&	m_triangleNodes;
+
+		NodeTriangleCallback& operator=(NodeTriangleCallback& other)
+		{
+			m_triangleNodes = other.m_triangleNodes;
+			return *this;
+		}
+		
+		NodeTriangleCallback(NodeArray&	triangleNodes)
+			:m_triangleNodes(triangleNodes)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		{
+			btOptimizedBvhNode node;
+			btVector3	aabbMin,aabbMax;
+			aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+			aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+			aabbMin.setMin(triangle[0]);
+			aabbMax.setMax(triangle[0]);
+			aabbMin.setMin(triangle[1]);
+			aabbMax.setMax(triangle[1]);
+			aabbMin.setMin(triangle[2]);
+			aabbMax.setMax(triangle[2]);
+
+			//with quantization?
+			node.m_aabbMinOrg = aabbMin;
+			node.m_aabbMaxOrg = aabbMax;
+
+			node.m_escapeIndex = -1;
+	
+			//for child nodes
+			node.m_subPart = partId;
+			node.m_triangleIndex = triangleIndex;
+			m_triangleNodes.push_back(node);
+		}
+	};
+	struct	QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
+	{
+		QuantizedNodeArray&	m_triangleNodes;
+		const btQuantizedBvh* m_optimizedTree; // for quantization
+
+		QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
+		{
+			m_triangleNodes = other.m_triangleNodes;
+			m_optimizedTree = other.m_optimizedTree;
+			return *this;
+		}
+
+		QuantizedNodeTriangleCallback(QuantizedNodeArray&	triangleNodes,const btQuantizedBvh* tree)
+			:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		{
+			// The partId and triangle index must fit in the same (positive) integer
+			btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
+			btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
+			//negative indices are reserved for escapeIndex
+			btAssert(triangleIndex>=0);
+
+			btQuantizedBvhNode node;
+			btVector3	aabbMin,aabbMax;
+			aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+			aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+			aabbMin.setMin(triangle[0]);
+			aabbMax.setMax(triangle[0]);
+			aabbMin.setMin(triangle[1]);
+			aabbMax.setMax(triangle[1]);
+			aabbMin.setMin(triangle[2]);
+			aabbMax.setMax(triangle[2]);
+
+			//PCK: add these checks for zero dimensions of aabb
+			const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
+			const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
+			if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
+			{
+				aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
+				aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
+			}
+			if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
+			{
+				aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
+				aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
+			}
+			if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
+			{
+				aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
+				aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
+			}
+
+			m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
+			m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
+
+			node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+
+			m_triangleNodes.push_back(node);
+		}
+	};
+	
+
+
+	int numLeafNodes = 0;
+
+	
+	if (m_useQuantization)
+	{
+
+		//initialize quantization values
+		setQuantizationValues(bvhAabbMin,bvhAabbMax);
+
+		QuantizedNodeTriangleCallback	callback(m_quantizedLeafNodes,this);
+
+	
+		triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
+
+		//now we have an array of leafnodes in m_leafNodes
+		numLeafNodes = m_quantizedLeafNodes.size();
+
+
+		m_quantizedContiguousNodes.resize(2*numLeafNodes);
+
+
+	} else
+	{
+		NodeTriangleCallback	callback(m_leafNodes);
+
+		btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+		btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+
+		triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+
+		//now we have an array of leafnodes in m_leafNodes
+		numLeafNodes = m_leafNodes.size();
+
+		m_contiguousNodes.resize(2*numLeafNodes);
+	}
+
+	m_curNodeIndex = 0;
+
+	buildTree(0,numLeafNodes);
+
+	///if the entire tree is small then subtree size, we need to create a header info for the tree
+	if(m_useQuantization && !m_SubtreeHeaders.size())
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
+		subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
+		subtree.m_rootNodeIndex = 0;
+		subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
+	}
+
+	//PCK: update the copy of the size
+	m_subtreeHeaderCount = m_SubtreeHeaders.size();
+
+	//PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
+	m_quantizedLeafNodes.clear();
+	m_leafNodes.clear();
+}
+
+
+
+
+void	btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	if (m_useQuantization)
+	{
+
+		setQuantizationValues(aabbMin,aabbMax);
+
+		updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
+
+		///now update all subtree headers
+
+		int i;
+		for (i=0;i<m_SubtreeHeaders.size();i++)
+		{
+			btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+			subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+		}
+
+	} else
+	{
+
+	}
+}
+
+
+
+
+void	btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	//incrementally initialize quantization values
+	btAssert(m_useQuantization);
+
+	btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
+	btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
+	btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
+
+	btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
+	btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
+	btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
+
+	///we should update all quantization values, using updateBvhNodes(meshInterface);
+	///but we only update chunks that overlap the given aabb
+	
+	unsigned short	quantizedQueryAabbMin[3];
+	unsigned short	quantizedQueryAabbMax[3];
+
+	quantize(&quantizedQueryAabbMin[0],aabbMin,0);
+	quantize(&quantizedQueryAabbMax[0],aabbMax,1);
+
+	int i;
+	for (i=0;i<this->m_SubtreeHeaders.size();i++)
+	{
+		btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+
+		//PCK: unsigned instead of bool
+		unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
+
+			subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+		}
+	}
+	
+}
+
+void	btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
+{
+	(void)index;
+
+	btAssert(m_useQuantization);
+
+	int curNodeSubPart=-1;
+
+	//get access info to trianglemesh data
+		const unsigned char *vertexbase = 0;
+		int numverts = 0;
+		PHY_ScalarType type = PHY_INTEGER;
+		int stride = 0;
+		const unsigned char *indexbase = 0;
+		int indexstride = 0;
+		int numfaces = 0;
+		PHY_ScalarType indicestype = PHY_INTEGER;
+
+		btVector3	triangleVerts[3];
+		btVector3	aabbMin,aabbMax;
+		const btVector3& meshScaling = meshInterface->getScaling();
+		
+		int i;
+		for (i=endNode-1;i>=firstNode;i--)
+		{
+
+
+			btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
+			if (curNode.isLeafNode())
+			{
+				//recalc aabb from triangle data
+				int nodeSubPart = curNode.getPartId();
+				int nodeTriangleIndex = curNode.getTriangleIndex();
+				if (nodeSubPart != curNodeSubPart)
+				{
+					if (curNodeSubPart >= 0)
+						meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
+					meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
+
+					curNodeSubPart = nodeSubPart;
+					btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
+				}
+				//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
+
+				unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
+				
+				
+				for (int j=2;j>=0;j--)
+				{
+					
+					int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+					btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
+#ifdef DEBUG_PATCH_COLORS
+					btVector3 mycolor = color[index&3];
+					graphicsbase[8] = mycolor.getX();
+					graphicsbase[9] = mycolor.getY();
+					graphicsbase[10] = mycolor.getZ();
+#endif //DEBUG_PATCH_COLORS
+
+
+					triangleVerts[j] = btVector3(
+						graphicsbase[0]*meshScaling.getX(),
+						graphicsbase[1]*meshScaling.getY(),
+						graphicsbase[2]*meshScaling.getZ());
+				}
+
+
+				
+				aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+				aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
+				aabbMin.setMin(triangleVerts[0]);
+				aabbMax.setMax(triangleVerts[0]);
+				aabbMin.setMin(triangleVerts[1]);
+				aabbMax.setMax(triangleVerts[1]);
+				aabbMin.setMin(triangleVerts[2]);
+				aabbMax.setMax(triangleVerts[2]);
+
+				quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
+				quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
+				
+			} else
+			{
+				//combine aabb from both children
+
+				btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
+				
+				btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
+					&m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
+				
+
+				{
+					for (int i=0;i<3;i++)
+					{
+						curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
+						if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
+							curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
+
+						curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
+						if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
+							curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
+					}
+				}
+			}
+
+		}
+
+		if (curNodeSubPart >= 0)
+			meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
+
+		
+}
+
+///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
+{
+	btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
+	
+	//we don't add additional data so just do a static upcast
+	return static_cast<btOptimizedBvh*>(bvh);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.h	(revision 1963)
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OPTIMIZED_BVH_H
+#define OPTIMIZED_BVH_H
+
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h"
+
+class btStridingMeshInterface;
+
+
+///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface.
+ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh
+{
+	
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+protected:
+
+public:
+
+	btOptimizedBvh();
+
+	virtual ~btOptimizedBvh();
+
+	void	build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax);
+
+	void	refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax);
+
+	void	refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax);
+
+	void	updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index);
+
+	/// Data buffer MUST be 16 byte aligned
+	virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
+	{
+		return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
+
+	}
+
+	///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
+	static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
+
+
+};
+
+
+#endif //OPTIMIZED_BVH_H
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp	(revision 1963)
@@ -0,0 +1,185 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+
+btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
+m_localAabbMin(1,1,1),
+m_localAabbMax(-1,-1,-1),
+m_isLocalAabbValid(false),
+m_optionalHull(0)
+{
+
+}
+
+
+btVector3	btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+{
+	int i;
+	btVector3 supVec(0,0,0);
+
+	btScalar maxDot(btScalar(-1e30));
+
+	btVector3 vec = vec0;
+	btScalar lenSqr = vec.length2();
+	if (lenSqr < btScalar(0.0001))
+	{
+		vec.setValue(1,0,0);
+	} else
+	{
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		vec *= rlen;
+	}
+
+	btVector3 vtx;
+	btScalar newDot;
+
+	for (i=0;i<getNumVertices();i++)
+	{
+		getVertex(i,vtx);
+		newDot = vec.dot(vtx);
+		if (newDot > maxDot)
+		{
+			maxDot = newDot;
+			supVec = vtx;
+		}
+	}
+
+	return supVec;
+
+}
+
+void	btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	int i;
+
+	btVector3 vtx;
+	btScalar newDot;
+
+	for (i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i][3] = btScalar(-1e30);
+	}
+
+	for (int j=0;j<numVectors;j++)
+	{
+	
+		const btVector3& vec = vectors[j];
+
+		for (i=0;i<getNumVertices();i++)
+		{
+			getVertex(i,vtx);
+			newDot = vec.dot(vtx);
+			if (newDot > supportVerticesOut[j][3])
+			{
+				//WARNING: don't swap next lines, the w component would get overwritten!
+				supportVerticesOut[j] = vtx;
+				supportVerticesOut[j][3] = newDot;
+			}
+		}
+	}
+}
+
+
+
+void	btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	//not yet, return box inertia
+
+	btScalar margin = getMargin();
+
+	btTransform ident;
+	ident.setIdentity();
+	btVector3 aabbMin,aabbMax;
+	getAabb(ident,aabbMin,aabbMax);
+	btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
+
+	btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
+	btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
+	btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+}
+
+
+
+void btPolyhedralConvexShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
+}
+
+
+
+void	btPolyhedralConvexShape::setLocalScaling(const btVector3& scaling)
+{
+	btConvexInternalShape::setLocalScaling(scaling);
+	recalcLocalAabb();
+}
+
+void	btPolyhedralConvexShape::recalcLocalAabb()
+{
+	m_isLocalAabbValid = true;
+	
+	#if 1
+	static const btVector3 _directions[] =
+	{
+		btVector3( 1.,  0.,  0.),
+		btVector3( 0.,  1.,  0.),
+		btVector3( 0.,  0.,  1.),
+		btVector3( -1., 0.,  0.),
+		btVector3( 0., -1.,  0.),
+		btVector3( 0.,  0., -1.)
+	};
+	
+	btVector3 _supporting[] =
+	{
+		btVector3( 0., 0., 0.),
+		btVector3( 0., 0., 0.),
+		btVector3( 0., 0., 0.),
+		btVector3( 0., 0., 0.),
+		btVector3( 0., 0., 0.),
+		btVector3( 0., 0., 0.)
+	};
+	
+	batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
+	
+	for ( int i = 0; i < 3; ++i )
+	{
+		m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
+		m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
+	}
+	
+	#else
+
+	for (int i=0;i<3;i++)
+	{
+		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		vec[i] = btScalar(1.);
+		btVector3 tmp = localGetSupportingVertex(vec);
+		m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+		vec[i] = btScalar(-1.);
+		tmp = localGetSupportingVertex(vec);
+		m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+	}
+	#endif
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h	(revision 1963)
@@ -0,0 +1,91 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BU_SHAPE
+#define BU_SHAPE
+
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "btConvexInternalShape.h"
+
+
+///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
+class btPolyhedralConvexShape : public btConvexInternalShape
+{
+
+protected:
+	btVector3	m_localAabbMin;
+	btVector3	m_localAabbMax;
+	bool		m_isLocalAabbValid;
+
+public:
+
+	btPolyhedralConvexShape();
+
+	//brute force implementations
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+	
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+	void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax)
+	{
+		m_isLocalAabbValid = true;
+		m_localAabbMin = aabbMin;
+		m_localAabbMax = aabbMax;
+	}
+
+	inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const
+	{
+		btAssert(m_isLocalAabbValid);
+		aabbMin = m_localAabbMin;
+		aabbMax = m_localAabbMax;
+	}
+
+	inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const
+	{
+
+		//lazy evaluation of local aabb
+		btAssert(m_isLocalAabbValid);
+		btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
+	}
+
+	
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+
+	void	recalcLocalAabb();
+
+	virtual int	getNumVertices() const = 0 ;
+	virtual int getNumEdges() const = 0;
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const = 0;
+	virtual void getVertex(int i,btPoint3& vtx) const = 0;
+	virtual int	getNumPlanes() const = 0;
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const = 0;
+//	virtual int getIndex(int i) const = 0 ; 
+
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const = 0;
+	
+	/// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
+	class	Hull*	m_optionalHull;
+
+};
+
+#endif //BU_SHAPE
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp	(revision 1963)
@@ -0,0 +1,120 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btScaledBvhTriangleMeshShape.h"
+
+btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling)
+:m_localScaling(localScaling),m_bvhTriMeshShape(childShape)
+{
+	m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE;
+}
+
+btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape()
+{
+}
+
+
+class btScaledTriangleCallback : public btTriangleCallback
+{
+	btTriangleCallback* m_originalCallback;
+
+	btVector3	m_localScaling;
+
+public:
+
+	btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling)
+		:m_originalCallback(originalCallback),
+		m_localScaling(localScaling)
+	{
+	}
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		btVector3 newTriangle[3];
+		newTriangle[0] = triangle[0]*m_localScaling;
+		newTriangle[1] = triangle[1]*m_localScaling;
+		newTriangle[2] = triangle[2]*m_localScaling;
+		m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex);
+	}
+};
+
+void	btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	btScaledTriangleCallback scaledCallback(callback,m_localScaling);
+	
+	btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ());
+	btVector3 scaledAabbMin,scaledAabbMax;
+
+	///support negative scaling
+	scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
+	scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
+	scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
+	
+	scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0];
+	scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1];
+	scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
+	
+	
+	m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax);
+}
+
+
+void	btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin();
+	btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax();
+
+	btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling;
+	btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling;
+
+	localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
+	localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
+	localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
+	localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
+	localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
+	localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
+
+	btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+	btScalar margin = m_bvhTriMeshShape->getMargin();
+	localHalfExtents += btVector3(margin,margin,margin);
+	btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+	
+	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+	btPoint3 center = trans(localCenter);
+
+	btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+		   abs_b[1].dot(localHalfExtents),
+		  abs_b[2].dot(localHalfExtents));
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+
+}
+
+void	btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling;
+}
+
+const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const
+{
+	return m_localScaling;
+}
+
+void	btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	///don't make this a movable object!
+//	btAssert(0);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h	(revision 1963)
@@ -0,0 +1,62 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+
+
+///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
+///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
+ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
+{
+	
+	
+	btVector3	m_localScaling;
+
+	btBvhTriangleMeshShape*	m_bvhTriMeshShape;
+
+public:
+
+
+	btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling);
+
+	virtual ~btScaledBvhTriangleMeshShape();
+
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	btBvhTriangleMeshShape*	getChildShape()
+	{
+		return m_bvhTriMeshShape;
+	}
+
+	const btBvhTriangleMeshShape*	getChildShape() const
+	{
+		return m_bvhTriMeshShape;
+	}
+
+	//debugging
+	virtual const char*	getName()const {return "SCALEDBVHTRIANGLEMESH";}
+
+};
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp	(revision 1963)
@@ -0,0 +1,164 @@
+/*
+btbtShapeHull implemented by John McCutchan.
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btShapeHull.h"
+#include "LinearMath/btConvexHull.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+
+static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+{
+	btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+	btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+	btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+	btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+	btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+	btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+	btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+	btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+	btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+	btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+	btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+	btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+	btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+	btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+	btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+	btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+	btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+	btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+	btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+	btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+	btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+	btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+	btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+	btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+	btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+	btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+	btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+	btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+	btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+	btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+	btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+	btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+	btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+	btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+	btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+	btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+	btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+	btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+	btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+	btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+	btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+	btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+btShapeHull::btShapeHull (const btConvexShape* shape)
+{
+	m_shape = shape;
+	m_vertices.clear ();
+	m_indices.clear();
+	m_numIndices = 0;
+}
+
+btShapeHull::~btShapeHull ()
+{
+	m_indices.clear();	
+	m_vertices.clear ();
+}
+
+bool
+btShapeHull::buildHull (btScalar /*margin*/)
+{
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+	{
+		int numPDA = m_shape->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				m_shape->getPreferredPenetrationDirection(i,norm);
+				btUnitSpherePoints[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	int i;
+	for (i = 0; i < numSampleDirections; i++)
+	{
+		supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
+	}
+
+	HullDesc hd;
+	hd.mFlags = QF_TRIANGLES;
+	hd.mVcount = static_cast<unsigned int>(numSampleDirections);
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	hd.mVertices = &supportPoints[0];
+	hd.mVertexStride = sizeof(btVector3);
+#else
+	hd.mVertices = &supportPoints[0];
+	hd.mVertexStride = sizeof (btVector3);
+#endif
+
+	HullLibrary hl;
+	HullResult hr;
+	if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
+	{
+		return false;
+	}
+
+	m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
+
+
+	for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
+	{
+		m_vertices[i] = hr.m_OutputVertices[i];
+	}
+	m_numIndices = hr.mNumIndices;
+	m_indices.resize(static_cast<int>(m_numIndices));
+	for (i = 0; i < static_cast<int>(m_numIndices); i++)
+	{
+		m_indices[i] = hr.m_Indices[i];
+	}
+
+	// free temporary hull result that we just copied
+	hl.ReleaseResult (hr);
+
+	return true;
+}
+
+int
+btShapeHull::numTriangles () const
+{
+	return static_cast<int>(m_numIndices / 3);
+}
+
+int
+btShapeHull::numVertices () const
+{
+	return m_vertices.size ();
+}
+
+int
+btShapeHull::numIndices () const
+{
+	return static_cast<int>(m_numIndices);
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btShapeHull.h	(revision 1963)
@@ -0,0 +1,56 @@
+/*
+btShapeHull implemented by John McCutchan.
+
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _SHAPE_HULL_H
+#define _SHAPE_HULL_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+
+///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices.
+///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object.
+///It approximates the convex hull using the supporting vertex of 42 directions.
+class btShapeHull
+{
+public:
+	btShapeHull (const btConvexShape* shape);
+	~btShapeHull ();
+
+	bool buildHull (btScalar margin);
+
+	int numTriangles () const;
+	int numVertices () const;
+	int numIndices () const;
+
+	const btVector3* getVertexPointer() const
+	{
+		return &m_vertices[0];
+	}
+	const unsigned int* getIndexPointer() const
+	{
+		return &m_indices[0];
+	}
+
+protected:
+	btAlignedObjectArray<btVector3> m_vertices;
+	btAlignedObjectArray<unsigned int> m_indices;
+	unsigned int m_numIndices;
+	const btConvexShape* m_shape;
+};
+
+#endif //_SHAPE_HULL_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.cpp	(revision 1963)
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btQuaternion.h"
+
+btVector3	btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	(void)vec;
+	return btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void	btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	(void)vectors;
+
+	for (int i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	}
+}
+
+
+btVector3	btSphereShape::localGetSupportingVertex(const btVector3& vec)const
+{
+	btVector3 supVertex;
+	supVertex = localGetSupportingVertexWithoutMargin(vec);
+
+	btVector3 vecnorm = vec;
+	if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
+	} 
+	vecnorm.normalize();
+	supVertex+= getMargin() * vecnorm;
+	return supVertex;
+}
+
+
+//broken due to scaling
+void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	const btVector3& center = t.getOrigin();
+	btVector3 extent(getMargin(),getMargin(),getMargin());
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+}
+
+
+
+void	btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
+	inertia.setValue(elem,elem,elem);
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h	(revision 1963)
@@ -0,0 +1,68 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_MINKOWSKI_H
+#define SPHERE_MINKOWSKI_H
+
+#include "btConvexInternalShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btSphereShape implements an implicit sphere, centered around a local origin with radius.
+ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape
+
+{
+	
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btSphereShape (btScalar radius) : btConvexInternalShape ()
+	{
+		m_shapeType = SPHERE_SHAPE_PROXYTYPE;
+		m_implicitShapeDimensions.setX(radius);
+		m_collisionMargin = radius;
+	}
+	
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+	//notice that the vectors should be unit length
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+	btScalar	getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
+
+	//debugging
+	virtual const char*	getName()const {return "SPHERE";}
+
+	virtual void	setMargin(btScalar margin)
+	{
+		btConvexInternalShape::setMargin(margin);
+	}
+	virtual btScalar	getMargin() const
+	{
+		//to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
+		//this means, non-uniform scaling is not supported anymore
+		return getRadius();
+	}
+
+
+};
+
+
+#endif //SPHERE_MINKOWSKI_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp	(revision 1963)
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btStaticPlaneShape.h"
+
+#include "LinearMath/btTransformUtil.h"
+
+
+btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
+: btConcaveShape (), m_planeNormal(planeNormal.normalized()),
+m_planeConstant(planeConstant),
+m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
+{
+	m_shapeType = STATIC_PLANE_PROXYTYPE;
+	//	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );
+}
+
+
+btStaticPlaneShape::~btStaticPlaneShape()
+{
+}
+
+
+
+void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	(void)t;
+	/*
+	btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30));
+
+	btVector3 center = m_planeNormal*m_planeConstant;
+	aabbMin = center + infvec*m_planeNormal;
+	aabbMax = aabbMin;
+	aabbMin.setMin(center - infvec*m_planeNormal);
+	aabbMax.setMax(center - infvec*m_planeNormal); 
+	*/
+
+	aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+
+}
+
+
+
+
+void	btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+
+	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+	btScalar radius = halfExtents.length();
+	btVector3 center = (aabbMax + aabbMin) * btScalar(0.5);
+	
+	//this is where the triangles are generated, given AABB and plane equation (normal/constant)
+
+	btVector3 tangentDir0,tangentDir1;
+
+	//tangentDir0/tangentDir1 can be precalculated
+	btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
+
+	btVector3 supVertex0,supVertex1;
+
+	btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
+	
+	btVector3 triangle[3];
+	triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+	triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
+	triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+
+	callback->processTriangle(triangle,0,0);
+
+	triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+	triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
+	triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+
+	callback->processTriangle(triangle,0,1);
+
+}
+
+void	btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	(void)mass;
+
+	//moving concave objects not supported
+	
+	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+void	btStaticPlaneShape::setLocalScaling(const btVector3& scaling)
+{
+	m_localScaling = scaling;
+}
+const btVector3& btStaticPlaneShape::getLocalScaling() const
+{
+	return m_localScaling;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.h	(revision 1963)
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef STATIC_PLANE_SHAPE_H
+#define STATIC_PLANE_SHAPE_H
+
+#include "btConcaveShape.h"
+
+
+///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane.
+class btStaticPlaneShape : public btConcaveShape
+{
+protected:
+	btVector3	m_localAabbMin;
+	btVector3	m_localAabbMax;
+	
+	btVector3	m_planeNormal;
+	btScalar      m_planeConstant;
+	btVector3	m_localScaling;
+
+public:
+	btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+
+	virtual ~btStaticPlaneShape();
+
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+	
+	const btVector3&	getPlaneNormal() const
+	{
+		return	m_planeNormal;
+	}
+
+	const btScalar&	getPlaneConstant() const
+	{
+		return	m_planeConstant;
+	}
+
+	//debugging
+	virtual const char*	getName()const {return "STATICPLANE";}
+
+
+};
+
+#endif //STATIC_PLANE_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp	(revision 1963)
@@ -0,0 +1,124 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btStridingMeshInterface.h"
+
+btStridingMeshInterface::~btStridingMeshInterface()
+{
+
+}
+
+
+void	btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	(void)aabbMin;
+	(void)aabbMax;
+	int numtotalphysicsverts = 0;
+	int part,graphicssubparts = getNumSubParts();
+	const unsigned char * vertexbase;
+	const unsigned char * indexbase;
+	int indexstride;
+	PHY_ScalarType type;
+	PHY_ScalarType gfxindextype;
+	int stride,numverts,numtriangles;
+	int gfxindex;
+	btVector3 triangle[3];
+	btScalar* graphicsbase;
+
+	btVector3 meshScaling = getScaling();
+
+	///if the number of parts is big, the performance might drop due to the innerloop switch on indextype
+	for (part=0;part<graphicssubparts ;part++)
+	{
+		getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
+		numtotalphysicsverts+=numtriangles*3; //upper bound
+
+		switch (gfxindextype)
+		{
+		case PHY_INTEGER:
+			{
+				for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+				{
+					unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride);
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+					triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+					triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+					triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
+					callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+				}
+				break;
+			}
+		case PHY_SHORT:
+			{
+				for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+				{
+					unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride);
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+					triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+					triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
+					graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+					triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),	graphicsbase[2]*meshScaling.getZ());
+					callback->internalProcessTriangleIndex(triangle,part,gfxindex);
+				}
+				break;
+			}
+		default:
+			btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
+		}
+
+		unLockReadOnlyVertexBase(part);
+	}
+}
+
+void	btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax)
+{
+
+	struct	AabbCalculationCallback : public btInternalTriangleIndexCallback
+	{
+		btVector3	m_aabbMin;
+		btVector3	m_aabbMax;
+
+		AabbCalculationCallback()
+		{
+			m_aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+			m_aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+		{
+			(void)partId;
+			(void)triangleIndex;
+
+			m_aabbMin.setMin(triangle[0]);
+			m_aabbMax.setMax(triangle[0]);
+			m_aabbMin.setMin(triangle[1]);
+			m_aabbMax.setMax(triangle[1]);
+			m_aabbMin.setMin(triangle[2]);
+			m_aabbMax.setMax(triangle[2]);
+		}
+	};
+
+		//first calculate the total aabb for all triangles
+	AabbCalculationCallback	aabbCallback;
+	aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);
+
+	aabbMin = aabbCallback.m_aabbMin;
+	aabbMax = aabbCallback.m_aabbMax;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.h	(revision 1963)
@@ -0,0 +1,95 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef STRIDING_MESHINTERFACE_H
+#define STRIDING_MESHINTERFACE_H
+
+#include "LinearMath/btVector3.h"
+#include "btTriangleCallback.h"
+
+/// PHY_ScalarType enumerates possible scalar types.
+/// See the btStridingMeshInterface for its use
+typedef enum PHY_ScalarType {
+	PHY_FLOAT,
+	PHY_DOUBLE,
+	PHY_INTEGER,
+	PHY_SHORT,
+	PHY_FIXEDPOINT88
+} PHY_ScalarType;
+
+///	The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes.
+/// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips.
+/// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
+class  btStridingMeshInterface
+{
+	protected:
+	
+		btVector3 m_scaling;
+
+	public:
+		btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.))
+		{
+
+		}
+
+		virtual ~btStridingMeshInterface();
+
+
+
+		virtual void	InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+		///brute force method to calculate aabb
+		void	calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax);
+
+		/// get read and write access to a subpart of a triangle mesh
+		/// this subpart has a continuous array of vertices and indices
+		/// in this way the mesh can be handled as chunks of memory with striding
+		/// very similar to OpenGL vertexarray support
+		/// make a call to unLockVertexBase when the read and write access is finished	
+		virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
+		
+		virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0;
+	
+		/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+		/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+		virtual void	unLockVertexBase(int subpart)=0;
+
+		virtual void	unLockReadOnlyVertexBase(int subpart) const=0;
+
+
+		/// getNumSubParts returns the number of seperate subparts
+		/// each subpart has a continuous array of vertices and indices
+		virtual int		getNumSubParts() const=0;
+
+		virtual void	preallocateVertices(int numverts)=0;
+		virtual void	preallocateIndices(int numindices)=0;
+
+		virtual bool	hasPremadeAabb() const { return false; }
+		virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const {}
+		virtual void	getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const {}
+
+		const btVector3&	getScaling() const {
+			return m_scaling;
+		}
+		void	setScaling(const btVector3& scaling)
+		{
+			m_scaling = scaling;
+		}
+
+	
+
+};
+
+#endif //STRIDING_MESHINTERFACE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.cpp	(revision 1963)
@@ -0,0 +1,200 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btTetrahedronShape.h"
+#include "LinearMath/btMatrix3x3.h"
+
+btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexShape (),
+m_numVertices(0)
+{
+	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0) : btPolyhedralConvexShape (),
+m_numVertices(0)
+{
+	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+	addVertex(pt0);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1) : btPolyhedralConvexShape (),
+m_numVertices(0)
+{
+	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+	addVertex(pt0);
+	addVertex(pt1);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2) : btPolyhedralConvexShape (),
+m_numVertices(0)
+{
+	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+	addVertex(pt0);
+	addVertex(pt1);
+	addVertex(pt2);
+}
+
+btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3) : btPolyhedralConvexShape (),
+m_numVertices(0)
+{
+	m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE;
+	addVertex(pt0);
+	addVertex(pt1);
+	addVertex(pt2);
+	addVertex(pt3);
+}
+
+
+
+
+
+void btBU_Simplex1to4::addVertex(const btPoint3& pt)
+{
+	m_vertices[m_numVertices++] = pt;
+
+	recalcLocalAabb();
+}
+
+
+int	btBU_Simplex1to4::getNumVertices() const
+{
+	return m_numVertices;
+}
+
+int btBU_Simplex1to4::getNumEdges() const
+{
+	//euler formula, F-E+V = 2, so E = F+V-2
+
+	switch (m_numVertices)
+	{
+	case 0:
+		return 0;
+	case 1: return 0;
+	case 2: return 1;
+	case 3: return 3;
+	case 4: return 6;
+
+
+	}
+
+	return 0;
+}
+
+void btBU_Simplex1to4::getEdge(int i,btPoint3& pa,btPoint3& pb) const
+{
+	
+    switch (m_numVertices)
+	{
+
+	case 2: 
+		pa = m_vertices[0];
+		pb = m_vertices[1];
+		break;
+	case 3:  
+		switch (i)
+		{
+		case 0:
+			pa = m_vertices[0];
+			pb = m_vertices[1];
+			break;
+		case 1:
+			pa = m_vertices[1];
+			pb = m_vertices[2];
+			break;
+		case 2:
+			pa = m_vertices[2];
+			pb = m_vertices[0];
+			break;
+
+		}
+		break;
+	case 4: 
+		switch (i)
+		{
+		case 0:
+			pa = m_vertices[0];
+			pb = m_vertices[1];
+			break;
+		case 1:
+			pa = m_vertices[1];
+			pb = m_vertices[2];
+			break;
+		case 2:
+			pa = m_vertices[2];
+			pb = m_vertices[0];
+			break;
+		case 3:
+			pa = m_vertices[0];
+			pb = m_vertices[3];
+			break;
+		case 4:
+			pa = m_vertices[1];
+			pb = m_vertices[3];
+			break;
+		case 5:
+			pa = m_vertices[2];
+			pb = m_vertices[3];
+			break;
+		}
+
+	}
+
+
+
+
+}
+
+void btBU_Simplex1to4::getVertex(int i,btPoint3& vtx) const
+{
+	vtx = m_vertices[i];
+}
+
+int	btBU_Simplex1to4::getNumPlanes() const
+{
+	switch (m_numVertices)
+	{
+	case 0:
+			return 0;
+	case 1:
+			return 0;
+	case 2:
+			return 0;
+	case 3:
+			return 2;
+	case 4:
+			return 4;
+	default:
+		{
+		}
+	}
+	return 0;
+}
+
+
+void btBU_Simplex1to4::getPlane(btVector3&, btPoint3& ,int ) const
+{
+	
+}
+
+int btBU_Simplex1to4::getIndex(int ) const
+{
+	return 0;
+}
+
+bool btBU_Simplex1to4::isInside(const btPoint3& ,btScalar ) const
+{
+	return false;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTetrahedronShape.h	(revision 1963)
@@ -0,0 +1,74 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BU_SIMPLEX_1TO4_SHAPE
+#define BU_SIMPLEX_1TO4_SHAPE
+
+
+#include "btPolyhedralConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+
+
+///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead.
+class btBU_Simplex1to4 : public btPolyhedralConvexShape
+{
+protected:
+
+	int	m_numVertices;
+	btPoint3	m_vertices[4];
+
+public:
+	btBU_Simplex1to4();
+
+	btBU_Simplex1to4(const btPoint3& pt0);
+	btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1);
+	btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2);
+	btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3);
+
+    
+	void	reset()
+	{
+		m_numVertices = 0;
+	}
+	
+
+
+	void addVertex(const btPoint3& pt);
+
+	//PolyhedralConvexShape interface
+
+	virtual int	getNumVertices() const;
+
+	virtual int getNumEdges() const;
+
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const;
+	
+	virtual void getVertex(int i,btPoint3& vtx) const;
+
+	virtual int	getNumPlanes() const;
+
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const;
+
+	virtual int getIndex(int i) const;
+
+	virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const;
+
+
+	///getName is for debugging
+	virtual const char*	getName()const { return "btBU_Simplex1to4";}
+
+};
+
+#endif //BU_SIMPLEX_1TO4_SHAPE
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.cpp	(revision 1963)
@@ -0,0 +1,35 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleBuffer.h"
+
+
+
+
+
+
+
+void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int  triangleIndex)
+{
+		btTriangle	tri;
+		tri.m_vertex0 = triangle[0];
+		tri.m_vertex1 = triangle[1];
+		tri.m_vertex2 = triangle[2];
+		tri.m_partId = partId;
+		tri.m_triangleIndex = triangleIndex;
+			
+		m_triangleBuffer.push_back(tri);
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleBuffer.h	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_TRIANGLE_BUFFER_H
+#define BT_TRIANGLE_BUFFER_H
+
+#include "btTriangleCallback.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct	btTriangle
+{
+	btVector3	m_vertex0;
+	btVector3	m_vertex1;
+	btVector3	m_vertex2;
+	int	m_partId;
+	int	m_triangleIndex;
+};
+
+///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
+///Example usage of this class:
+///			btTriangleBuffer	triBuf;
+///			concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
+///			for (int i=0;i<triBuf.getNumTriangles();i++)
+///			{
+///				const btTriangle& tri = triBuf.getTriangle(i);
+///				//do something useful here with the triangle
+///			}
+class btTriangleBuffer : public btTriangleCallback
+{
+
+	btAlignedObjectArray<btTriangle>	m_triangleBuffer;
+	
+public:
+
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+	
+	int	getNumTriangles() const
+	{
+		return int(m_triangleBuffer.size());
+	}
+	
+	const btTriangle&	getTriangle(int index) const
+	{
+		return m_triangleBuffer[index];
+	}
+
+	void	clearBuffer()
+	{
+		m_triangleBuffer.clear();
+	}
+	
+};
+
+
+#endif //BT_TRIANGLE_BUFFER_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.cpp	(revision 1963)
@@ -0,0 +1,28 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleCallback.h"
+
+btTriangleCallback::~btTriangleCallback()
+{
+
+}
+
+
+btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback()
+{
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleCallback.h	(revision 1963)
@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TRIANGLE_CALLBACK_H
+#define TRIANGLE_CALLBACK_H
+
+#include "LinearMath/btVector3.h"
+
+
+///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles.
+///This callback is called by processAllTriangles for all btConcaveShape derived class, such as  btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape.
+class btTriangleCallback
+{
+public:
+
+	virtual ~btTriangleCallback();
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0;
+};
+
+class btInternalTriangleIndexCallback
+{
+public:
+
+	virtual ~btInternalTriangleIndexCallback();
+	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex) = 0;
+};
+
+
+
+#endif //TRIANGLE_CALLBACK_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp	(revision 1963)
@@ -0,0 +1,97 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleIndexVertexArray.h"
+
+btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride)
+: m_hasAabb(0)
+{
+	btIndexedMesh mesh;
+
+	mesh.m_numTriangles = numTriangles;
+	mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase;
+	mesh.m_triangleIndexStride = triangleIndexStride;
+	mesh.m_numVertices = numVertices;
+	mesh.m_vertexBase = (const unsigned char *)vertexBase;
+	mesh.m_vertexStride = vertexStride;
+
+	addIndexedMesh(mesh);
+
+}
+
+btTriangleIndexVertexArray::~btTriangleIndexVertexArray()
+{
+
+}
+
+void	btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+{
+	btAssert(subpart< getNumSubParts() );
+
+	btIndexedMesh& mesh = m_indexedMeshes[subpart];
+
+	numverts = mesh.m_numVertices;
+	(*vertexbase) = (unsigned char *) mesh.m_vertexBase;
+	#ifdef BT_USE_DOUBLE_PRECISION
+	type = PHY_DOUBLE;
+	#else
+	type = PHY_FLOAT;
+	#endif
+	vertexStride = mesh.m_vertexStride;
+
+	numfaces = mesh.m_numTriangles;
+
+	(*indexbase) = (unsigned char *)mesh.m_triangleIndexBase;
+	indexstride = mesh.m_triangleIndexStride;
+	indicestype = mesh.m_indexType;
+}
+
+void	btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+	const btIndexedMesh& mesh = m_indexedMeshes[subpart];
+
+	numverts = mesh.m_numVertices;
+	(*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
+	#ifdef BT_USE_DOUBLE_PRECISION
+	type = PHY_DOUBLE;
+	#else
+	type = PHY_FLOAT;
+	#endif
+	vertexStride = mesh.m_vertexStride;
+
+	numfaces = mesh.m_numTriangles;
+	(*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase;
+	indexstride = mesh.m_triangleIndexStride;
+	indicestype = mesh.m_indexType;
+}
+
+bool	btTriangleIndexVertexArray::hasPremadeAabb() const
+{
+	return (m_hasAabb == 1);
+}
+
+void	btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax )
+{
+	m_aabbMin = aabbMin;
+	m_aabbMax = aabbMax;
+	m_hasAabb = 1; // this is intentionally an int see notes in header
+}
+
+void	btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const
+{
+	*aabbMin = m_aabbMin;
+	*aabbMax = m_aabbMax;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h	(revision 1963)
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
+
+#include "btStridingMeshInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btScalar.h"
+
+
+///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh.
+///Instead of the number of indices, we pass the number of triangles.
+ATTRIBUTE_ALIGNED16( struct)	btIndexedMesh
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int			m_numTriangles;
+	const unsigned char *		m_triangleIndexBase;
+	int			m_triangleIndexStride;
+	int			m_numVertices;
+	const unsigned char *		m_vertexBase;
+	int			m_vertexStride;
+	// The index type is set when adding an indexed mesh to the
+	// btTriangleIndexVertexArray, do not set it manually
+	PHY_ScalarType		m_indexType;
+	int			pad;
+}
+;
+
+
+typedef btAlignedObjectArray<btIndexedMesh>	IndexedMeshArray;
+
+///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays.
+///Additional meshes can be added using addIndexedMesh
+///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
+///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
+ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface
+{
+protected:
+	IndexedMeshArray	m_indexedMeshes;
+	int m_pad[2];
+	int m_hasAabb; // using int instead of bool to maintain alignment
+	btVector3 m_aabbMin;
+	btVector3 m_aabbMax;
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btTriangleIndexVertexArray() : m_hasAabb(0)
+	{
+	}
+
+	virtual ~btTriangleIndexVertexArray();
+
+	//just to be backwards compatible
+	btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride);
+	
+	void	addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER)
+	{
+		m_indexedMeshes.push_back(mesh);
+		m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType;
+	}
+	
+	
+	virtual void	getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+
+	virtual void	getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
+	/// unLockVertexBase finishes the access to a subpart of the triangle mesh
+	/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+	virtual void	unLockVertexBase(int subpart) {(void)subpart;}
+
+	virtual void	unLockReadOnlyVertexBase(int subpart) const {(void)subpart;}
+
+	/// getNumSubParts returns the number of seperate subparts
+	/// each subpart has a continuous array of vertices and indices
+	virtual int		getNumSubParts() const { 
+		return (int)m_indexedMeshes.size();
+	}
+
+	IndexedMeshArray&	getIndexedMeshArray()
+	{
+		return m_indexedMeshes;
+	}
+
+	const IndexedMeshArray&	getIndexedMeshArray() const
+	{
+		return m_indexedMeshes;
+	}
+
+	virtual void	preallocateVertices(int numverts){(void) numverts;}
+	virtual void	preallocateIndices(int numindices){(void) numindices;}
+
+	virtual bool	hasPremadeAabb() const;
+	virtual void	setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax );
+	virtual void	getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
+
+}
+;
+
+#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp	(revision 1963)
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was created by Alex Silverman
+
+#include "btTriangleIndexVertexMaterialArray.h"
+
+btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
+                                   int numVertices,btScalar* vertexBase,int vertexStride,
+                                   int numMaterials, unsigned char* materialBase, int materialStride,
+                                   int* triangleMaterialsBase, int materialIndexStride) :
+btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride)
+{
+    btMaterialProperties mat;
+
+    mat.m_numMaterials = numMaterials;
+    mat.m_materialBase = materialBase;
+    mat.m_materialStride = materialStride;
+#ifdef BT_USE_DOUBLE_PRECISION
+    mat.m_materialType = PHY_DOUBLE;
+#else
+    mat.m_materialType = PHY_FLOAT;
+#endif
+
+    mat.m_numTriangles = numTriangles;
+    mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase;
+    mat.m_triangleMaterialStride = materialIndexStride;
+    mat.m_triangleType = PHY_INTEGER;
+
+    addMaterialProperties(mat);
+}
+
+
+void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+                                   unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+{
+    btAssert(subpart< getNumSubParts() );
+
+    btMaterialProperties& mats = m_materials[subpart];
+
+    numMaterials = mats.m_numMaterials;
+    (*materialBase) = (unsigned char *) mats.m_materialBase;
+#ifdef BT_USE_DOUBLE_PRECISION
+    materialType = PHY_DOUBLE;
+#else
+    materialType = PHY_FLOAT;
+#endif
+    materialStride = mats.m_materialStride;
+
+    numTriangles = mats.m_numTriangles;
+    (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase;
+    triangleMaterialStride = mats.m_triangleMaterialStride;
+    triangleType = mats.m_triangleType;
+}
+
+void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+                                           const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart)
+{
+    btMaterialProperties& mats = m_materials[subpart];
+
+    numMaterials = mats.m_numMaterials;
+    (*materialBase) = (const unsigned char *) mats.m_materialBase;
+#ifdef BT_USE_DOUBLE_PRECISION
+    materialType = PHY_DOUBLE;
+#else
+    materialType = PHY_FLOAT;
+#endif
+    materialStride = mats.m_materialStride;
+
+    numTriangles = mats.m_numTriangles;
+    (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase;
+    triangleMaterialStride = mats.m_triangleMaterialStride;
+    triangleType = mats.m_triangleType;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h	(revision 1963)
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was created by Alex Silverman
+
+#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
+#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
+
+#include "btTriangleIndexVertexArray.h"
+
+
+ATTRIBUTE_ALIGNED16( struct)	btMaterialProperties
+{
+    ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
+    int m_numMaterials;
+    const unsigned char * m_materialBase;
+    int m_materialStride;
+    PHY_ScalarType m_materialType;
+    ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're
+    ///                           padding the structure, it can be reproduced at no real cost
+    ///m_triangleMaterials =====> 1 integer value makes up one entry
+    ///                           eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
+    int m_numTriangles; 
+    const unsigned char * m_triangleMaterialsBase;
+    int m_triangleMaterialStride;
+    ///m_triangleType <========== Automatically set in addMaterialProperties
+    PHY_ScalarType m_triangleType;
+};
+
+typedef btAlignedObjectArray<btMaterialProperties>	MaterialArray;
+
+///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray
+///The addition of a material array allows for the utilization of the partID and
+///triangleIndex that are returned in the ContactAddedCallback.  As with
+///TriangleIndexVertexArray, no duplicate is made of the material data, so it
+///is the users responsibility to maintain the array during the lifetime of the
+///TriangleIndexVertexMaterialArray.
+ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
+{
+protected:
+    MaterialArray       m_materials;
+		
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btTriangleIndexVertexMaterialArray()
+	{
+	}
+
+    btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
+        int numVertices,btScalar* vertexBase,int vertexStride,
+        int numMaterials, unsigned char* materialBase, int materialStride,
+        int* triangleMaterialsBase, int materialIndexStride);
+
+    virtual ~btTriangleIndexVertexMaterialArray() {}
+
+    void	addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
+    {
+        m_materials.push_back(mat);
+        m_materials[m_materials.size()-1].m_triangleType = triangleType;
+    }
+
+    virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+        unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0);
+
+    virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride,
+        const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
+
+}
+;
+
+#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp	(revision 1963)
@@ -0,0 +1,133 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleMesh.h"
+
+
+
+btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices)
+:m_use32bitIndices(use32bitIndices),
+m_use4componentVertices(use4componentVertices),
+m_weldingThreshold(0.0)
+{
+	btIndexedMesh meshIndex;
+	meshIndex.m_numTriangles = 0;
+	meshIndex.m_numVertices = 0;
+	meshIndex.m_indexType = PHY_INTEGER;
+	meshIndex.m_triangleIndexBase = 0;
+	meshIndex.m_triangleIndexStride = 3*sizeof(int);
+	meshIndex.m_vertexBase = 0;
+	meshIndex.m_vertexStride = sizeof(btVector3);
+	m_indexedMeshes.push_back(meshIndex);
+
+	if (m_use32bitIndices)
+	{
+		m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
+		m_indexedMeshes[0].m_indexType = PHY_INTEGER;
+		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
+	} else
+	{
+		m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+		m_indexedMeshes[0].m_indexType = PHY_SHORT;
+		m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
+	}
+
+	if (m_use4componentVertices)
+	{
+		m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
+		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
+		m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
+	} else
+	{
+		m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
+		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
+		m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
+	}
+
+
+}
+
+void	btTriangleMesh::addIndex(int index)
+{
+	if (m_use32bitIndices)
+	{
+		m_32bitIndices.push_back(index);
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
+	} else
+	{
+		m_16bitIndices.push_back(index);
+		m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+	}
+}
+
+int	btTriangleMesh::findOrAddVertex(const btVector3& vertex)
+{
+	//return index of new/existing vertex
+	//todo: could use acceleration structure for this
+	if (m_use4componentVertices)
+	{
+		for (int i=0;i< m_4componentVertices.size();i++)
+		{
+			if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
+			{
+				return i;
+			}
+		}
+		m_indexedMeshes[0].m_numVertices++;
+		m_4componentVertices.push_back(vertex);
+		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
+
+		return m_4componentVertices.size()-1;
+		
+	} else
+	{
+		
+		for (int i=0;i< m_3componentVertices.size();i+=3)
+		{
+			btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
+			if ((vtx-vertex).length2() <= m_weldingThreshold)
+			{
+				return i/3;
+			}
+		}
+		m_3componentVertices.push_back(vertex.getX());
+		m_3componentVertices.push_back(vertex.getY());
+		m_3componentVertices.push_back(vertex.getZ());
+		m_indexedMeshes[0].m_numVertices++;
+		m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
+		return (m_3componentVertices.size()/3)-1;
+	}
+
+}
+		
+void	btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2)
+{
+	m_indexedMeshes[0].m_numTriangles++;
+		
+	addIndex(findOrAddVertex(vertex0));
+	addIndex(findOrAddVertex(vertex1));
+	addIndex(findOrAddVertex(vertex2));
+}
+
+int btTriangleMesh::getNumTriangles() const
+{
+	if (m_use32bitIndices)
+	{
+		return m_32bitIndices.size() / 3;
+	}
+	return m_16bitIndices.size() / 3;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.h	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef TRIANGLE_MESH_H
+#define TRIANGLE_MESH_H
+
+#include "btTriangleIndexVertexArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///The btTriangleMesh class is a convenience class derived from btTriangleIndexVertexArray, that provides storage for a concave triangle mesh. It can be used as data for the btBvhTriangleMeshShape.
+///It allows either 32bit or 16bit indices, and 4 (x-y-z-w) or 3 (x-y-z) component vertices.
+///If you want to share triangle/index data between graphics mesh and collision mesh (btBvhTriangleMeshShape), you can directly use btTriangleIndexVertexArray or derive your own class from btStridingMeshInterface.
+///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same.
+///It has a brute-force option to weld together closeby vertices.
+class btTriangleMesh : public btTriangleIndexVertexArray
+{
+	btAlignedObjectArray<btVector3>	m_4componentVertices;
+	btAlignedObjectArray<float>		m_3componentVertices;
+
+	btAlignedObjectArray<unsigned int>		m_32bitIndices;
+	btAlignedObjectArray<unsigned short int>		m_16bitIndices;
+	bool	m_use32bitIndices;
+	bool	m_use4componentVertices;
+	
+
+	public:
+		btScalar	m_weldingThreshold;
+
+		btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true);
+
+		int		findOrAddVertex(const btVector3& vertex);
+		void	addIndex(int index);
+
+		bool	getUse32bitIndices() const
+		{
+			return m_use32bitIndices;
+		}
+
+		bool	getUse4componentVertices() const
+		{
+			return m_use4componentVertices;
+		}
+		
+		void	addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2);
+		
+		int getNumTriangles() const;
+
+		virtual void	preallocateVertices(int numverts){(void) numverts;}
+		virtual void	preallocateIndices(int numindices){(void) numindices;}
+
+		
+};
+
+#endif //TRIANGLE_MESH_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp	(revision 1963)
@@ -0,0 +1,209 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleMeshShape.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+#include "btStridingMeshInterface.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+
+btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface)
+: btConcaveShape (), m_meshInterface(meshInterface)
+{
+	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	if(meshInterface->hasPremadeAabb())
+	{
+		meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax);
+	}
+	else
+	{
+		recalcLocalAabb();
+	}
+}
+
+
+btTriangleMeshShape::~btTriangleMeshShape()
+{
+		
+}
+
+
+
+
+void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+
+	btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
+	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+	
+	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+	btPoint3 center = trans(localCenter);
+
+	btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+		   abs_b[1].dot(localHalfExtents),
+		  abs_b[2].dot(localHalfExtents));
+	aabbMin = center - extent;
+	aabbMax = center + extent;
+
+
+}
+
+void	btTriangleMeshShape::recalcLocalAabb()
+{
+	for (int i=0;i<3;i++)
+	{
+		btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
+		vec[i] = btScalar(1.);
+		btVector3 tmp = localGetSupportingVertex(vec);
+		m_localAabbMax[i] = tmp[i]+m_collisionMargin;
+		vec[i] = btScalar(-1.);
+		tmp = localGetSupportingVertex(vec);
+		m_localAabbMin[i] = tmp[i]-m_collisionMargin;
+	}
+}
+
+
+
+class SupportVertexCallback : public btTriangleCallback
+{
+
+	btVector3 m_supportVertexLocal;
+public:
+
+	btTransform	m_worldTrans;
+	btScalar m_maxDot;
+	btVector3 m_supportVecLocal;
+
+	SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
+		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30))
+		
+	{
+		m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
+	}
+
+	virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex)
+	{
+		(void)partId;
+		(void)triangleIndex;
+		for (int i=0;i<3;i++)
+		{
+			btScalar dot = m_supportVecLocal.dot(triangle[i]);
+			if (dot > m_maxDot)
+			{
+				m_maxDot = dot;
+				m_supportVertexLocal = triangle[i];
+			}
+		}
+	}
+
+	btVector3 GetSupportVertexWorldSpace()
+	{
+		return m_worldTrans(m_supportVertexLocal);
+	}
+
+	btVector3	GetSupportVertexLocal()
+	{
+		return m_supportVertexLocal;
+	}
+
+};
+
+	
+void btTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+	m_meshInterface->setScaling(scaling);
+	recalcLocalAabb();
+}
+
+const btVector3& btTriangleMeshShape::getLocalScaling() const
+{
+	return m_meshInterface->getScaling();
+}
+
+
+
+
+
+
+//#define DEBUG_TRIANGLE_MESH
+
+
+
+void	btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+		struct FilteredCallback : public btInternalTriangleIndexCallback
+	{
+		btTriangleCallback* m_callback;
+		btVector3 m_aabbMin;
+		btVector3 m_aabbMax;
+
+		FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax)
+			:m_callback(callback),
+			m_aabbMin(aabbMin),
+			m_aabbMax(aabbMax)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
+		{
+			if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
+			{
+				//check aabb in triangle-space, before doing this
+				m_callback->processTriangle(triangle,partId,triangleIndex);
+			}
+			
+		}
+
+	};
+
+	FilteredCallback filterCallback(callback,aabbMin,aabbMax);
+
+	m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax);
+}
+
+
+
+
+
+void	btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	(void)mass;
+	//moving concave objects not supported
+	btAssert(0);
+	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+}
+
+
+btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
+{
+	btVector3 supportVertex;
+
+	btTransform ident;
+	ident.setIdentity();
+
+	SupportVertexCallback supportCallback(vec,ident);
+
+	btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	
+	processAllTriangles(&supportCallback,-aabbMax,aabbMax);
+		
+	supportVertex = supportCallback.GetSupportVertexLocal();
+
+	return supportVertex;
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h	(revision 1963)
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TRIANGLE_MESH_SHAPE_H
+#define TRIANGLE_MESH_SHAPE_H
+
+#include "btConcaveShape.h"
+#include "btStridingMeshInterface.h"
+
+
+///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead.
+class btTriangleMeshShape : public btConcaveShape
+{
+protected:
+	btVector3	m_localAabbMin;
+	btVector3	m_localAabbMax;
+	btStridingMeshInterface* m_meshInterface;
+
+	///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class.
+	///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead!
+	btTriangleMeshShape(btStridingMeshInterface* meshInterface);
+
+public:
+
+	virtual ~btTriangleMeshShape();
+
+	virtual btVector3 localGetSupportingVertex(const btVector3& vec) const;
+
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	{
+		assert(0);
+		return localGetSupportingVertex(vec);
+	}
+
+	void	recalcLocalAabb();
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	virtual const btVector3& getLocalScaling() const;
+	
+	btStridingMeshInterface* getMeshInterface()
+	{
+		return m_meshInterface;
+	}
+
+	const btStridingMeshInterface* getMeshInterface() const
+	{
+		return m_meshInterface;
+	}
+
+	const btVector3& getLocalAabbMin() const
+	{
+		return m_localAabbMin;
+	}
+	const btVector3& getLocalAabbMax() const
+	{
+		return m_localAabbMax;
+	}
+
+
+
+	//debugging
+	virtual const char*	getName()const {return "TRIANGLEMESH";}
+
+
+};
+
+#endif //TRIANGLE_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btTriangleShape.h	(revision 1963)
@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef OBB_TRIANGLE_MINKOWSKI_H
+#define OBB_TRIANGLE_MINKOWSKI_H
+
+#include "btConvexShape.h"
+#include "btBoxShape.h"
+
+class btTriangleShape : public btPolyhedralConvexShape
+{
+
+
+public:
+
+	btVector3	m_vertices1[3];
+
+	virtual int getNumVertices() const
+	{
+		return 3;
+	}
+
+	const btVector3& getVertexPtr(int index) const
+	{
+		return m_vertices1[index];
+	}
+	virtual void getVertex(int index,btVector3& vert) const
+	{
+		vert = m_vertices1[index];
+	}
+
+	virtual int getNumEdges() const
+	{
+		return 3;
+	}
+	
+	virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const
+	{
+		getVertex(i,pa);
+		getVertex((i+1)%3,pb);
+	}
+
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const 
+	{
+//		btAssert(0);
+		getAabbSlow(t,aabbMin,aabbMax);
+	}
+
+	btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const 
+	{
+		btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+	  	return m_vertices1[dots.maxAxis()];
+
+	}
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	{
+		for (int i=0;i<numVectors;i++)
+		{
+			const btVector3& dir = vectors[i];
+			btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2]));
+  			supportVerticesOut[i] = m_vertices1[dots.maxAxis()];
+		}
+
+	}
+
+
+
+	btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape ()
+    {
+		m_shapeType = TRIANGLE_SHAPE_PROXYTYPE;
+        m_vertices1[0] = p0;
+        m_vertices1[1] = p1;
+        m_vertices1[2] = p2;
+    }
+
+
+	virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const
+	{
+		getPlaneEquation(i,planeNormal,planeSupport);
+	}
+
+	virtual int	getNumPlanes() const
+	{
+		return 1;
+	}
+
+	void calcNormal(btVector3& normal) const
+	{
+		normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		normal.normalize();
+	}
+
+	virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const
+	{
+		(void)i;
+		calcNormal(planeNormal);
+		planeSupport = m_vertices1[0];
+	}
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
+	{
+		(void)mass;
+		btAssert(0);
+		inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	}
+
+		virtual	bool isInside(const btPoint3& pt,btScalar tolerance) const
+	{
+		btVector3 normal;
+		calcNormal(normal);
+		//distance to plane
+		btScalar dist = pt.dot(normal);
+		btScalar planeconst = m_vertices1[0].dot(normal);
+		dist -= planeconst;
+		if (dist >= -tolerance && dist <= tolerance)
+		{
+			//inside check on edge-planes
+			int i;
+			for (i=0;i<3;i++)
+			{
+				btPoint3 pa,pb;
+				getEdge(i,pa,pb);
+				btVector3 edge = pb-pa;
+				btVector3 edgeNormal = edge.cross(normal);
+				edgeNormal.normalize();
+				btScalar dist = pt.dot( edgeNormal);
+				btScalar edgeConst = pa.dot(edgeNormal);
+				dist -= edgeConst;
+				if (dist < -tolerance)
+					return false;
+			}
+			
+			return true;
+		}
+
+		return false;
+	}
+		//debugging
+		virtual const char*	getName()const
+		{
+			return "Triangle";
+		}
+
+		virtual int		getNumPreferredPenetrationDirections() const
+		{
+			return 2;
+		}
+		
+		virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+		{
+			calcNormal(penetrationVector);
+			if (index)
+				penetrationVector *= btScalar(-1.);
+		}
+
+
+};
+
+#endif //OBB_TRIANGLE_MINKOWSKI_H
+
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.cpp	(revision 1963)
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btUniformScalingShape.h"
+
+btUniformScalingShape::btUniformScalingShape(	btConvexShape* convexChildShape,btScalar uniformScalingFactor):
+btConvexShape (), m_childConvexShape(convexChildShape),
+m_uniformScalingFactor(uniformScalingFactor)
+{
+	m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE;
+}
+	
+btUniformScalingShape::~btUniformScalingShape()
+{
+}
+	
+
+btVector3	btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+{
+	btVector3 tmpVertex;
+	tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec);
+	return tmpVertex*m_uniformScalingFactor;
+}
+
+void	btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+{
+	m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
+	int i;
+	for (i=0;i<numVectors;i++)
+	{
+		supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor;
+	}
+}
+
+
+btVector3	btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const
+{
+	btVector3 tmpVertex;
+	tmpVertex = m_childConvexShape->localGetSupportingVertex(vec);
+	return tmpVertex*m_uniformScalingFactor;
+}
+
+
+void	btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+
+	///this linear upscaling is not realistic, but we don't deal with large mass ratios...
+	btVector3 tmpInertia;
+	m_childConvexShape->calculateLocalInertia(mass,tmpInertia);
+	inertia = tmpInertia * m_uniformScalingFactor;
+}
+
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void btUniformScalingShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	m_childConvexShape->getAabb(t,aabbMin,aabbMax);
+	btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5);
+	btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor;
+
+	aabbMin = aabbCenter - scaledAabbHalfExtends;
+	aabbMax = aabbCenter + scaledAabbHalfExtends;
+
+}
+
+void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax);
+	btVector3 aabbCenter = (aabbMax+aabbMin)*btScalar(0.5);
+	btVector3 scaledAabbHalfExtends = (aabbMax-aabbMin)*btScalar(0.5)*m_uniformScalingFactor;
+
+	aabbMin = aabbCenter - scaledAabbHalfExtends;
+	aabbMax = aabbCenter + scaledAabbHalfExtends;
+}
+
+void	btUniformScalingShape::setLocalScaling(const btVector3& scaling) 
+{
+	m_childConvexShape->setLocalScaling(scaling);
+}
+
+const btVector3& btUniformScalingShape::getLocalScaling() const
+{
+	return m_childConvexShape->getLocalScaling();
+}
+
+void	btUniformScalingShape::setMargin(btScalar margin)
+{
+	m_childConvexShape->setMargin(margin);
+}
+btScalar	btUniformScalingShape::getMargin() const
+{
+	return m_childConvexShape->getMargin() * m_uniformScalingFactor;
+}
+
+int		btUniformScalingShape::getNumPreferredPenetrationDirections() const
+{
+	return m_childConvexShape->getNumPreferredPenetrationDirections();
+}
+	
+void	btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+{
+	m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector);
+}
Index: code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/CollisionShapes/btUniformScalingShape.h	(revision 1963)
@@ -0,0 +1,87 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_UNIFORM_SCALING_SHAPE_H
+#define BT_UNIFORM_SCALING_SHAPE_H
+
+#include "btConvexShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+
+///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way.
+///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it.
+class btUniformScalingShape : public btConvexShape
+{
+	btConvexShape*	m_childConvexShape;
+
+	btScalar	m_uniformScalingFactor;
+	
+	public:
+	
+	btUniformScalingShape(	btConvexShape* convexChildShape, btScalar uniformScalingFactor);
+	
+	virtual ~btUniformScalingShape();
+	
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec)const;
+
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	btScalar	getUniformScalingFactor() const
+	{
+		return m_uniformScalingFactor;
+	}
+
+	btConvexShape*	getChildShape() 
+	{
+		return m_childConvexShape;
+	}
+
+	const btConvexShape*	getChildShape() const
+	{
+		return m_childConvexShape;
+	}
+
+	virtual const char*	getName()const 
+	{
+		return "UniformScalingShape";
+	}
+	
+
+
+	///////////////////////////
+
+
+	///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+	void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+
+	virtual void	setLocalScaling(const btVector3& scaling) ;
+	virtual const btVector3& getLocalScaling() const ;
+
+	virtual void	setMargin(btScalar margin);
+	virtual btScalar	getMargin() const;
+
+	virtual int		getNumPreferredPenetrationDirections() const;
+	
+	virtual void	getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const;
+
+
+};
+
+#endif //BT_UNIFORM_SCALING_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btBoxCollision.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btBoxCollision.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btBoxCollision.h	(revision 1963)
@@ -0,0 +1,651 @@
+#ifndef BT_BOX_COLLISION_H_INCLUDED
+#define BT_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+/*! \defgroup BOUND_AABB_OPERATIONS
+*/
+//! @{
+
+///Swap numbers
+#define BT_SWAP_NUMBERS(a,b){ \
+    a = a+b; \
+    b = a-b; \
+    a = a-b; \
+}\
+
+
+#define BT_MAX(a,b) (a<b?b:a)
+#define BT_MIN(a,b) (a>b?b:a)
+
+#define BT_GREATER(x, y)	fabsf(x) > (y)
+
+#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
+#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
+
+
+
+
+
+
+enum eBT_PLANE_INTERSECTION_TYPE
+{
+	BT_CONST_BACK_PLANE = 0,
+	BT_CONST_COLLIDE_PLANE,
+	BT_CONST_FRONT_PLANE
+};
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, const btVector3 & extend,
+//	int dir_index0,
+//	int dir_index1
+//	int component_index0,
+//	int component_index1)
+//{
+//	// dir coords are -z and y
+//
+//	const btScalar dir0 = -edge[dir_index0];
+//	const btScalar dir1 = edge[dir_index1];
+//	btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+//	btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+//	//find minmax
+//	if(pmin>pmax)
+//	{
+//		BT_SWAP_NUMBERS(pmin,pmax);
+//	}
+//	//find extends
+//	const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+//					extend[component_index1] * absolute_edge[dir_index1];
+//
+//	if(pmin>rad || -rad>pmax) return false;
+//	return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+	const btScalar dir0 = -edge[i_dir_0];\
+	const btScalar dir1 = edge[i_dir_1];\
+	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+	if(pmin>pmax)\
+	{\
+		BT_SWAP_NUMBERS(pmin,pmax); \
+	}\
+	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+	if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+//! Returns the dot product between a vec3f and the col of a matrix
+SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
+const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
+{
+	return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
+}
+
+
+//!  Class for transforming a model1 to the space of model0
+ATTRIBUTE_ALIGNED16	(class) BT_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+	SIMD_FORCE_INLINE void calc_absolute_matrix()
+	{
+//		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+//		m_AR[0] = vepsi + m_R1to0[0].absolute();
+//		m_AR[1] = vepsi + m_R1to0[1].absolute();
+//		m_AR[2] = vepsi + m_R1to0[2].absolute();
+
+		int i,j;
+
+        for(i=0;i<3;i++)
+        {
+            for(j=0;j<3;j++ )
+            {
+            	m_AR[i][j] = 1e-6f + fabsf(m_R1to0[i][j]);
+            }
+        }
+
+	}
+
+	BT_BOX_BOX_TRANSFORM_CACHE()
+	{
+	}
+
+
+
+	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	{
+
+		btTransform temp_trans = trans0.inverse();
+		temp_trans = temp_trans * trans1;
+
+		m_T1to0 = temp_trans.getOrigin();
+		m_R1to0 = temp_trans.getBasis();
+
+
+		calc_absolute_matrix();
+	}
+
+	//! Calcs the full invertion of the matrices. Useful for scaling matrices
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	{
+		m_R1to0 = trans0.getBasis().inverse();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
+	{
+		return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+			m_R1to0[1].dot(point) + m_T1to0.y(),
+			m_R1to0[2].dot(point) + m_T1to0.z());
+	}
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+ATTRIBUTE_ALIGNED16	(class) btAABB
+{
+public:
+	btVector3 m_min;
+	btVector3 m_max;
+
+	btAABB()
+	{}
+
+
+	btAABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	btAABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3,
+			 btScalar margin)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	btAABB(const btAABB &other):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+	}
+
+	btAABB(const btAABB &other,btScalar margin ):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void invalidate()
+	{
+		m_min[0] = SIMD_INFINITY;
+		m_min[1] = SIMD_INFINITY;
+		m_min[2] = SIMD_INFINITY;
+		m_max[0] = -SIMD_INFINITY;
+		m_max[1] = -SIMD_INFINITY;
+		m_max[2] = -SIMD_INFINITY;
+	}
+
+	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
+	{
+		m_min[0] = other.m_min[0] - margin;
+		m_min[1] = other.m_min[1] - margin;
+		m_min[2] = other.m_min[2] - margin;
+
+		m_max[0] = other.m_max[0] + margin;
+		m_max[1] = other.m_max[1] + margin;
+		m_max[2] = other.m_max[2] + margin;
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle_margin(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3, btScalar margin)
+	{
+		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans(center);
+
+		btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ 				 extends.dot(trans.getBasis().getRow(1).absolute()),
+				 extends.dot(trans.getBasis().getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans.transform(center);
+
+		btVector3 textends(extends.dot(trans.m_R1to0.getRow(0).absolute()),
+ 				 extends.dot(trans.m_R1to0.getRow(1).absolute()),
+				 extends.dot(trans.m_R1to0.getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+	//! Merges a Box
+	SIMD_FORCE_INLINE void merge(const btAABB & box)
+	{
+		m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
+		m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
+		m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
+
+		m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
+		m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
+		m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
+	}
+
+	//! Merges a point
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	{
+		m_min[0] = BT_MIN(m_min[0],point[0]);
+		m_min[1] = BT_MIN(m_min[1],point[1]);
+		m_min[2] = BT_MIN(m_min[2],point[2]);
+
+		m_max[0] = BT_MAX(m_max[0],point[0]);
+		m_max[1] = BT_MAX(m_max[1],point[1]);
+		m_max[2] = BT_MAX(m_max[2],point[2]);
+	}
+
+	//! Gets the extend and center
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	{
+		center = (m_max+m_min)*0.5f;
+		extend = m_max - center;
+	}
+
+	//! Finds the intersecting box between this box and the other.
+	SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
+	{
+		intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
+		intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
+		intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
+
+		intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
+		intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
+		intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
+	}
+
+
+	SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
+	{
+		if(m_min[0] > other.m_max[0] ||
+		   m_max[0] < other.m_min[0] ||
+		   m_min[1] > other.m_max[1] ||
+		   m_max[1] < other.m_min[1] ||
+		   m_min[2] > other.m_max[2] ||
+		   m_max[2] < other.m_min[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+	/*! \brief Finds the Ray intersection parameter.
+	\param aabb Aligned box
+	\param vorigin A vec3f with the origin of the ray
+	\param vdir A vec3f with the direction of the ray
+	*/
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
+	{
+		btVector3 extents,center;
+		this->get_center_extend(center,extents);;
+
+		btScalar Dx = vorigin[0] - center[0];
+		if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		btScalar Dy = vorigin[1] - center[1];
+		if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		btScalar Dz = vorigin[2] - center[2];
+		if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
+
+
+		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		f = vdir[2] * Dx - vdir[0] * Dz;
+		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		f = vdir[0] * Dy - vdir[1] * Dx;
+		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		return true;
+	}
+
+
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extend = m_max-center;
+
+		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fMaximumExtent = extend.dot(direction.absolute());
+		vmin = _fOrigin - _fMaximumExtent;
+		vmax = _fOrigin + _fMaximumExtent;
+	}
+
+	SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+	{
+		btScalar _fmin,_fmax;
+		this->projection_interval(plane,_fmin,_fmax);
+
+		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		{
+			return BT_CONST_BACK_PLANE; // 0
+		}
+
+		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		{
+			return BT_CONST_COLLIDE_PLANE; //1
+		}
+		return BT_CONST_FRONT_PLANE;//2
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
+	{
+		btAABB tbox = box;
+		tbox.appy_transform(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
+		const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
+	{
+		btAABB tbox = box;
+		tbox.appy_transform_trans_cache(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	//! transcache is the transformation cache from box to this AABB
+	SIMD_FORCE_INLINE bool overlapping_trans_cache(
+		const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
+	{
+
+		//Taken from OPCODE
+		btVector3 ea,eb;//extends
+		btVector3 ca,cb;//extends
+		get_center_extend(ca,ea);
+		box.get_center_extend(cb,eb);
+
+
+		btVector3 T;
+		btScalar t,t2;
+		int i;
+
+		// Class I : A's basis vectors
+		for(i=0;i<3;i++)
+		{
+			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			t = transcache.m_AR[i].dot(eb) + ea[i];
+			if(BT_GREATER(T[i], t))	return false;
+		}
+		// Class II : B's basis vectors
+		for(i=0;i<3;i++)
+		{
+			t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
+			t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
+			if(BT_GREATER(t,t2))	return false;
+		}
+		// Class III : 9 cross products
+		if(fulltest)
+		{
+			int j,m,n,o,p,q,r;
+			for(i=0;i<3;i++)
+			{
+				m = (i+1)%3;
+				n = (i+2)%3;
+				o = i==0?1:0;
+				p = i==2?1:2;
+				for(j=0;j<3;j++)
+				{
+					q = j==2?1:2;
+					r = j==0?1:0;
+					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+					if(BT_GREATER(t,t2))	return false;
+				}
+			}
+		}
+		return true;
+	}
+
+	//! Simple test for planes.
+	SIMD_FORCE_INLINE bool collide_plane(
+		const btVector4 & plane) const
+	{
+		eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+		return (classify == BT_CONST_COLLIDE_PLANE);
+	}
+
+	//! test for a triangle, with edges
+	SIMD_FORCE_INLINE bool collide_triangle_exact(
+		const btVector3 & p1,
+		const btVector3 & p2,
+		const btVector3 & p3,
+		const btVector4 & triangle_plane) const
+	{
+		if(!collide_plane(triangle_plane)) return false;
+
+		btVector3 center,extends;
+		this->get_center_extend(center,extends);
+
+		const btVector3 v1(p1 - center);
+		const btVector3 v2(p2 - center);
+		const btVector3 v3(p3 - center);
+
+		//First axis
+		btVector3 diff(v2 - v1);
+		btVector3 abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+		diff = v3 - v2;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+		diff = v1 - v3;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+		return true;
+	}
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	return true;
+}
+
+
+//! @}
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btClipPolygon.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btClipPolygon.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btClipPolygon.h	(revision 1963)
@@ -0,0 +1,186 @@
+#ifndef BT_CLIP_POLYGON_H_INCLUDED
+#define BT_CLIP_POLYGON_H_INCLUDED
+
+/*! \file btClipPolygon.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btGeometryUtil.h"
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
+{
+	return point.dot(plane) - plane[3];
+}
+
+/*! Vector blending
+Takes two vectors a, b, blends them together*/
+SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
+{
+	vr = (1-blend_factor)*va + blend_factor*vb;
+}
+
+//! This function calcs the distance from a 3D plane
+SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
+						const btVector3 & point0,
+						const btVector3 & point1,
+						btScalar dist0,
+						btScalar dist1,
+						btVector3 * clipped,
+						int & clipped_count)
+{
+	bool _prevclassif = (dist0>SIMD_EPSILON);
+	bool _classif = (dist1>SIMD_EPSILON);
+	if(_classif!=_prevclassif)
+	{
+		btScalar blendfactor = -dist0/(dist1-dist0);
+		bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
+		clipped_count++;
+	}
+	if(!_classif)
+	{
+		clipped[clipped_count] = point1;
+		clipped_count++;
+	}
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_polygon(
+						const btVector4 & plane,
+						const btVector3 * polygon_points,
+						int polygon_point_count,
+						btVector3 * clipped)
+{
+    int clipped_count = 0;
+
+
+    //clip first point
+	btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
+	if(!(firstdist>SIMD_EPSILON))
+	{
+		clipped[clipped_count] = polygon_points[0];
+		clipped_count++;
+	}
+
+	btScalar olddist = firstdist;
+	for(int i=1;i<polygon_point_count;i++)
+	{
+		btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);
+
+		bt_plane_clip_polygon_collect(
+						polygon_points[i-1],polygon_points[i],
+						olddist,
+						dist,
+						clipped,
+						clipped_count);
+
+
+		olddist = dist;
+	}
+
+	//RETURN TO FIRST  point
+
+	bt_plane_clip_polygon_collect(
+					polygon_points[polygon_point_count-1],polygon_points[0],
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\param clipped must be an array of 16 points.
+*\return The count of the clipped counts
+*/
+SIMD_FORCE_INLINE int bt_plane_clip_triangle(
+						const btVector4 & plane,
+						const btVector3 & point0,
+						const btVector3 & point1,
+						const btVector3& point2,
+						btVector3 * clipped // an allocated array of 16 points at least
+						)
+{
+    int clipped_count = 0;
+
+    //clip first point0
+	btScalar firstdist = bt_distance_point_plane(plane,point0);;
+	if(!(firstdist>SIMD_EPSILON))
+	{
+		clipped[clipped_count] = point0;
+		clipped_count++;
+	}
+
+	// point 1
+	btScalar olddist = firstdist;
+	btScalar dist = bt_distance_point_plane(plane,point1);
+
+	bt_plane_clip_polygon_collect(
+					point0,point1,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+
+	olddist = dist;
+
+
+	// point 2
+	dist = bt_distance_point_plane(plane,point2);
+
+	bt_plane_clip_polygon_collect(
+					point1,point2,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+	olddist = dist;
+
+
+
+	//RETURN TO FIRST  point0
+	bt_plane_clip_polygon_collect(
+					point2,point0,
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.cpp	(revision 1963)
@@ -0,0 +1,181 @@
+
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btContactProcessing.h"
+
+#define MAX_COINCIDENT 8
+
+struct CONTACT_KEY_TOKEN
+{
+	unsigned int m_key;
+	int m_value;
+	CONTACT_KEY_TOKEN()
+    {
+    }
+
+    CONTACT_KEY_TOKEN(unsigned int key,int token)
+    {
+    	m_key = key;
+    	m_value =  token;
+    }
+
+    CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
+    {
+    	m_key = rtoken.m_key;
+    	m_value = rtoken.m_value;
+    }
+
+    inline bool operator <(const CONTACT_KEY_TOKEN& other) const
+	{
+		return (m_key < other.m_key);
+	}
+
+	inline bool operator >(const CONTACT_KEY_TOKEN& other) const
+	{
+		return (m_key > other.m_key);
+	}
+
+};
+
+class CONTACT_KEY_TOKEN_COMP
+{
+	public:
+
+		bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b )
+		{
+			return ( a < b );
+		}
+};
+
+
+void btContactArray::merge_contacts(
+	const btContactArray & contacts, bool normal_contact_average)
+{
+	clear();
+
+	int i;
+	if(contacts.size()==0) return;
+
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts[0]);
+		return;
+	}
+
+	btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
+
+	keycontacts.reserve(contacts.size());
+
+	//fill key contacts
+
+	for ( i = 0;i<contacts.size() ;i++ )
+	{
+		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
+	}
+
+	//sort keys
+	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
+
+	// Merge contacts
+	int coincident_count=0;
+	btVector3 coincident_normals[MAX_COINCIDENT];
+
+	unsigned int last_key = keycontacts[0].m_key;
+	unsigned int key = 0;
+
+	push_back(contacts[keycontacts[0].m_value]);
+
+	BT_CONTACT * pcontact = &(*this)[0];
+
+	for( i=1;i<keycontacts.size();i++)
+	{
+	    key = keycontacts[i].m_key;
+		const BT_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+		if(last_key ==  key)//same points
+		{
+			//merge contact
+			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			{
+				*pcontact = *scontact;
+                coincident_count = 0;
+			}
+			else if(normal_contact_average)
+			{
+				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+                {
+                    if(coincident_count<MAX_COINCIDENT)
+                    {
+                    	coincident_normals[coincident_count] = scontact->m_normal;
+                        coincident_count++;
+                    }
+                }
+			}
+		}
+		else
+		{//add new contact
+
+		    if(normal_contact_average && coincident_count>0)
+		    {
+		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
+		        coincident_count = 0;
+		    }
+
+		    push_back(*scontact);
+		    pcontact = &(*this)[this->size()-1];
+        }
+		last_key = key;
+	}
+}
+
+void btContactArray::merge_contacts_unique(const btContactArray & contacts)
+{
+	clear();
+
+	if(contacts.size()==0) return;
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts[0]);
+		return;
+	}
+
+	BT_CONTACT average_contact = contacts[0];
+
+	for (int i=1;i<contacts.size() ;i++ )
+	{
+		average_contact.m_point += contacts[i].m_point;
+		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+	}
+
+	//divide
+	btScalar divide_average = 1.0f/((btScalar)contacts.size());
+
+	average_contact.m_point *= divide_average;
+
+	average_contact.m_normal *= divide_average;
+
+	average_contact.m_depth = average_contact.m_normal.length();
+
+	average_contact.m_normal /= average_contact.m_depth;
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btContactProcessing.h	(revision 1963)
@@ -0,0 +1,149 @@
+#ifndef BT_CONTACT_H_INCLUDED
+#define BT_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btTriangleShapeEx.h"
+
+
+/*! \defgroup CONTACTS
+\brief
+Functions for managing and sorting contacts resulting from a collision query.
+*/
+//! @{
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+/// Structure for collision results
+class BT_CONTACT
+{
+public:
+    btVector3 m_point;
+    btVector3 m_normal;
+    btScalar m_depth;//Positive value indicates interpenetration
+    btScalar m_distance;//Padding not for use
+    int m_feature1;//Face number
+    int m_feature2;//Face number
+public:
+    BT_CONTACT()
+    {
+    }
+
+    BT_CONTACT(const BT_CONTACT & contact):
+				m_point(contact.m_point),
+				m_normal(contact.m_normal),
+				m_depth(contact.m_depth),
+				m_feature1(contact.m_feature1),
+				m_feature2(contact.m_feature2)
+    {
+    }
+
+    BT_CONTACT(const btVector3 &point,const btVector3 & normal,
+    	 			btScalar depth, int feature1, int feature2):
+				m_point(point),
+				m_normal(normal),
+				m_depth(depth),
+				m_feature1(feature1),
+				m_feature2(feature2)
+    {
+    }
+
+	//! Calcs key for coord classification
+    SIMD_FORCE_INLINE unsigned int calc_key_contact() const
+    {
+    	int _coords[] = {
+    		(int)(m_point[0]*1000.0f+1.0f),
+    		(int)(m_point[1]*1333.0f),
+    		(int)(m_point[2]*2133.0f+3.0f)};
+		unsigned int _hash=0;
+		unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp)<<4;
+		_uitmp++;
+		_hash += (*_uitmp)<<8;
+		return _hash;
+    }
+
+    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
+    {
+    	btVector3 vec_sum(m_normal);
+		for(int i=0;i<normal_count;i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		btScalar vec_sum_len = vec_sum.length2();
+		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+		//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum/btSqrt(vec_sum_len);
+    }
+
+};
+
+
+class btContactArray:public btAlignedObjectArray<BT_CONTACT>
+{
+public:
+	btContactArray()
+	{
+		reserve(64);
+	}
+
+	SIMD_FORCE_INLINE void push_contact(
+		const btVector3 &point,const btVector3 & normal,
+		btScalar depth, int feature1, int feature2)
+	{
+		push_back( BT_CONTACT(point,normal,depth,feature1,feature2) );
+	}
+
+	SIMD_FORCE_INLINE void push_triangle_contacts(
+		const BT_TRIANGLE_CONTACT & tricontact,
+		int feature1,int feature2)
+	{
+		for(int i = 0;i<tricontact.m_point_count ;i++ )
+		{
+			push_contact(
+				tricontact.m_points[i],
+				tricontact.m_separating_normal,
+				tricontact.m_penetration_depth,feature1,feature2);
+		}
+	}
+
+	void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);
+
+	void merge_contacts_unique(const btContactArray & contacts);
+};
+
+//! @}
+#endif // GIM_CONTACT_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp	(revision 1963)
@@ -0,0 +1,499 @@
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btGImpactBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_tree_clock;
+
+float g_accum_tree_collision_time = 0;
+int g_count_traversing = 0;
+
+
+void bt_begin_gim02_tree_time()
+{
+	g_tree_clock.reset();
+}
+
+void bt_end_gim02_tree_time()
+{
+	g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
+	g_count_traversing++;
+}
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactBvh::getAverageTreeCollisionTime()
+{
+	if(g_count_traversing == 0) return 0;
+
+	float avgtime = g_accum_tree_collision_time;
+	avgtime /= (float)g_count_traversing;
+
+	g_accum_tree_collision_time = 0;
+	g_count_traversing = 0;
+	return avgtime;
+
+//	float avgtime = g_count_traversing;
+//	g_count_traversing = 0;
+//	return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btBvhTree /////////////////////////////////
+
+int btBvhTree::_calc_splitting_axis(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+
+	int i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+int btBvhTree::_sort_and_calc_splitting_index(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	int endIndex, int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	splitValue = means[splitAxis];
+
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			//swapLeafNodes(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	int rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+
+}
+
+
+void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+	int curIndex = m_num_nodes;
+	m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if ((endIndex-startIndex)==1)
+	{
+	    //We have a leaf node
+	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+		return;
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,
+			splitIndex//split axis
+			);
+
+
+	//calc this node bounding box
+
+	btAABB node_bound;
+	node_bound.invalidate();
+
+	for (int i=startIndex;i<endIndex;i++)
+	{
+		node_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	setNodeBound(curIndex,node_bound);
+
+
+	//build left branch
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+	//build right branch
+	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btBvhTree::build_tree(
+	BT_BVH_DATA_ARRAY & primitive_boxes)
+{
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactBvh
+
+void btGImpactBvh::refit()
+{
+	int nodecount = getNodeCount();
+	while(nodecount--)
+	{
+		if(isLeafNode(nodecount))
+		{
+			btAABB leafbox;
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+			setNodeBound(nodecount,leafbox);
+		}
+		else
+		{
+			//const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+			//get left bound
+			btAABB bound;
+			bound.invalidate();
+
+			btAABB temp_box;
+
+			int child_node = getLeftNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			child_node = getRightNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			setNodeBound(nodecount,bound);
+		}
+	}
+}
+
+//! this rebuild the entire set
+void btGImpactBvh::buildSet()
+{
+	//obtain primitive boxes
+	BT_BVH_DATA_ARRAY primitive_boxes;
+	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	{
+		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+		 primitive_boxes[i].m_data = i;
+	}
+
+	m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.has_collision(box);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData(curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactBvh::rayQuery(
+	const btVector3 & ray_dir,const btVector3 & ray_origin ,
+	btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData( curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+SIMD_FORCE_INLINE bool _node_collision(
+	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0 ,int node1, bool complete_primitive_tests)
+{
+	btAABB box0;
+	boxset0->getNodeBound(node0,box0);
+	btAABB box1;
+	boxset1->getNodeBound(node1,box1);
+
+	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+//	box1.appy_transform_trans_cache(trans_cache_1to0);
+//	return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_collision_pairs_recursive(
+	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
+	btPairSet * collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+	if( _node_collision(
+		boxset0,boxset1,trans_cache_1to0,
+		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+	if(boxset0->isLeafNode(node0))
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+			// collision result
+			collision_pairs->push_pair(
+				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+			return;
+		}
+		else
+		{
+
+			//collide left recursive
+
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getLeftNode(node1),false);
+
+			//collide right recursive
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getRightNode(node1),false);
+
+
+		}
+	}
+	else
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+
+			//collide left recursive
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getLeftNode(node0),node1,false);
+
+
+			//collide right recursive
+
+			_find_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getRightNode(node0),node1,false);
+
+
+		}
+		else
+		{
+			//collide left0 left1
+
+
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide left0 right1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+			//collide right0 left1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide right0 right1
+
+			_find_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+		}// else if node1 is not a leaf
+	}// else if node0 is not a leaf
+}
+
+
+void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
+		btGImpactBvh * boxset1, const btTransform & trans1,
+		btPairSet & collision_pairs)
+{
+
+	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+	bt_begin_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+	_find_collision_pairs_recursive(
+		boxset0,boxset1,
+		&collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+	bt_end_gim02_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactBvh.h	(revision 1963)
@@ -0,0 +1,401 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btBoxCollision.h"
+#include "btTriangleShapeEx.h"
+
+
+
+/*! \defgroup BOX_TREES
+
+
+
+*/
+//! @{
+
+
+//! Overlapping pair
+struct BT_PAIR
+{
+    int m_index1;
+    int m_index2;
+    BT_PAIR()
+    {}
+
+    BT_PAIR(const BT_PAIR & p)
+    {
+    	m_index1 = p.m_index1;
+    	m_index2 = p.m_index2;
+	}
+
+	BT_PAIR(int index1, int index2)
+    {
+    	m_index1 = index1;
+    	m_index2 = index2;
+	}
+};
+
+//! A pairset array
+class btPairSet: public btAlignedObjectArray<BT_PAIR>
+{
+public:
+	btPairSet()
+	{
+		reserve(32);
+	}
+	inline void push_pair(int index1,int index2)
+	{
+		push_back(BT_PAIR(index1,index2));
+	}
+
+	inline void push_pair_inv(int index1,int index2)
+	{
+		push_back(BT_PAIR(index2,index1));
+	}
+};
+
+
+
+struct BT_BVH_DATA
+{
+	btAABB m_bound;
+	int m_data;
+};
+
+//! Node Structure for trees
+class BT_BVH_TREE_NODE
+{
+public:
+	btAABB m_bound;
+protected:
+	int	m_escapeIndexOrDataIndex;
+public:
+	BT_BVH_TREE_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+};
+
+
+class BT_BVH_DATA_ARRAY:public btAlignedObjectArray<BT_BVH_DATA>
+{
+};
+
+
+class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<BT_BVH_TREE_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btBvhTree
+{
+protected:
+	int m_num_nodes;
+	BT_BVH_TREE_NODE_ARRAY m_node_array;
+protected:
+	int _sort_and_calc_splitting_index(
+		BT_BVH_DATA_ARRAY & primitive_boxes,
+		 int startIndex,  int endIndex, int splitAxis);
+
+	int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+
+	void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+public:
+	btBvhTree()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes);
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_node_array[nodeindex].isLeafNode();
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getDataIndex();
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	{
+		bound = m_node_array[nodeindex].m_bound;
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_node_array[nodeindex].m_bound = bound;
+	}
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return nodeindex+1;
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	{
+		return &m_node_array[index];
+	}
+
+	//!@}
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class btPrimitiveManagerBase
+{
+public:
+
+	//! determines if this manager consist on only triangles, which special case will be optimized
+	virtual bool is_trimesh() const = 0;
+	virtual int get_primitive_count() const = 0;
+	virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
+	//! retrieves only the points of the triangle, and the collision margin
+	virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
+};
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactBvh
+{
+protected:
+	btBvhTree m_box_tree;
+	btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+	//stackless refit
+	void refit();
+public:
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactBvh()
+	{
+		m_primitive_manager = NULL;
+	}
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	{
+		btAABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	void buildSet();
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	{
+		btAABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		btAlignedObjectArray<int> & collided_results) const;
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager->is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return m_box_tree.getLeftNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		return m_box_tree.getRightNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_box_tree.getEscapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	{
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+
+	SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const
+	{
+		return m_box_tree.get_node_pointer(index);
+	}
+
+//! @}
+
+	static float getAverageTreeCollisionTime();
+
+
+	static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
+		btGImpactBvh * boxset2, const btTransform & trans2,
+		btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,896 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+Author: Francisco Len Nßjera
+Concave-Concave Collision
+
+*/
+
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "btGImpactCollisionAlgorithm.h"
+#include "btContactProcessing.h"
+#include "LinearMath/btQuickprof.h"
+
+
+//! Class for accessing the plane equation
+class btPlaneShape : public btStaticPlaneShape
+{
+public:
+
+	btPlaneShape(const btVector3& v, float f)
+		:btStaticPlaneShape(v,f)
+	{
+	}
+
+	void get_plane_equation(btVector4 &equation)
+	{
+		equation[0] = m_planeNormal[0];
+		equation[1] = m_planeNormal[1];
+		equation[2] = m_planeNormal[2];
+		equation[3] = m_planeConstant;
+	}
+
+
+	void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation)
+	{
+		equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal);
+		equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal);
+		equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal);
+		equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant;
+	}
+};
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+#ifdef TRI_COLLISION_PROFILING
+
+btClock g_triangle_clock;
+
+float g_accum_triangle_collision_time = 0;
+int g_count_triangle_collision = 0;
+
+void bt_begin_gim02_tri_time()
+{
+	g_triangle_clock.reset();
+}
+
+void bt_end_gim02_tri_time()
+{
+	g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
+	g_count_triangle_collision++;
+}
+#endif //TRI_COLLISION_PROFILING
+//! Retrieving shapes shapes
+/*!
+Declared here due of insuficent space on Pool allocators
+*/
+//!@{
+class GIM_ShapeRetriever
+{
+public:
+	btGImpactShapeInterface * m_gim_shape;
+	btTriangleShapeEx m_trishape;
+	btTetrahedronShapeEx m_tetrashape;
+
+public:
+	class ChildShapeRetriever
+	{
+	public:
+		GIM_ShapeRetriever * m_parent;
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			return m_parent->m_gim_shape->getChildShape(index);
+		}
+	};
+
+	class TriangleShapeRetriever:public ChildShapeRetriever
+	{
+	public:
+
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape);
+			return &m_parent->m_trishape;
+		}
+	};
+
+	class TetraShapeRetriever:public ChildShapeRetriever
+	{
+	public:
+
+		virtual btCollisionShape * getChildShape(int index)
+		{
+			m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape);
+			return &m_parent->m_tetrashape;
+		}
+	};
+public:
+	ChildShapeRetriever m_child_retriever;
+	TriangleShapeRetriever m_tri_retriever;
+	TetraShapeRetriever  m_tetra_retriever;
+	ChildShapeRetriever * m_current_retriever;
+
+	GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape)
+	{
+		m_gim_shape = gim_shape;
+		//select retriever
+		if(m_gim_shape->needsRetrieveTriangles())
+		{
+			m_current_retriever = &m_tri_retriever;
+		}
+		else if(m_gim_shape->needsRetrieveTetrahedrons())
+		{
+			m_current_retriever = &m_tetra_retriever;
+		}
+		else
+		{
+			m_current_retriever = &m_child_retriever;
+		}
+
+		m_current_retriever->m_parent = this;
+	}
+
+	btCollisionShape * getChildShape(int index)
+	{
+		return m_current_retriever->getChildShape(index);
+	}
+
+
+};
+
+
+
+//!@}
+
+
+#ifdef TRI_COLLISION_PROFILING
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime()
+{
+	return btGImpactBoxSet::getAverageTreeCollisionTime();
+
+}
+
+//! Gets the average time in miliseconds of triangle collisions
+float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime()
+{
+	if(g_count_triangle_collision == 0) return 0;
+
+	float avgtime = g_accum_triangle_collision_time;
+	avgtime /= (float)g_count_triangle_collision;
+
+	g_accum_triangle_collision_time = 0;
+	g_count_triangle_collision = 0;
+
+	return avgtime;
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+
+
+btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+: btCollisionAlgorithm(ci)
+{
+	m_manifoldPtr = NULL;
+	m_convex_algorithm = NULL;
+}
+
+btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm()
+{
+	clearCache();
+}
+
+
+
+
+
+void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0,
+				btCollisionObject * body1,
+				const btVector3 & point,
+				const btVector3 & normal,
+				btScalar distance)
+{
+	m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+	checkManifold(body0,body1);
+	m_resultOut->addContactPoint(normal,point,distance);
+}
+
+
+void btGImpactCollisionAlgorithm::shape_vs_shape_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1)
+{
+
+	btCollisionShape* tmpShape0 = body0->getCollisionShape();
+	btCollisionShape* tmpShape1 = body1->getCollisionShape();
+	
+	body0->internalSetTemporaryCollisionShape(shape0);
+	body1->internalSetTemporaryCollisionShape(shape1);
+
+	{
+		btCollisionAlgorithm* algor = newAlgorithm(body0,body1);
+		// post :	checkManifold is called
+
+		m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+
+		algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+		algor->~btCollisionAlgorithm();
+		m_dispatcher->freeCollisionAlgorithm(algor);
+	}
+
+	body0->internalSetTemporaryCollisionShape(tmpShape0);
+	body1->internalSetTemporaryCollisionShape(tmpShape1);
+}
+
+void btGImpactCollisionAlgorithm::convex_vs_convex_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1)
+{
+
+	btCollisionShape* tmpShape0 = body0->getCollisionShape();
+	btCollisionShape* tmpShape1 = body1->getCollisionShape();
+	
+	body0->internalSetTemporaryCollisionShape(shape0);
+	body1->internalSetTemporaryCollisionShape(shape1);
+
+
+	m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1);
+
+	checkConvexAlgorithm(body0,body1);
+	m_convex_algorithm->processCollision(body0,body1,*m_dispatchInfo,m_resultOut);
+
+	body0->internalSetTemporaryCollisionShape(tmpShape0);
+	body1->internalSetTemporaryCollisionShape(tmpShape1);
+
+}
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1,btPairSet & pairset)
+{
+	if(shape0->hasBoxSet() && shape1->hasBoxSet())
+	{
+		btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset);
+	}
+	else
+	{
+		btAABB boxshape0;
+		btAABB boxshape1;
+		int i = shape0->getNumChildShapes();
+
+		while(i--)
+		{
+			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+			int j = shape1->getNumChildShapes();
+			while(j--)
+			{
+				shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max);
+
+				if(boxshape1.has_collision(boxshape0))
+				{
+					pairset.push_pair(i,j);
+				}
+			}
+		}
+	}
+
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,
+					  btAlignedObjectArray<int> & collided_primitives)
+{
+
+	btAABB boxshape;
+
+
+	if(shape0->hasBoxSet())
+	{
+		btTransform trans1to0 = trans0.inverse();
+		trans1to0 *= trans1;
+
+		shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);
+
+		shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
+	}
+	else
+	{
+		shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);
+
+		btAABB boxshape0;
+		int i = shape0->getNumChildShapes();
+
+		while(i--)
+		{
+			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);
+
+			if(boxshape.has_collision(boxshape0))
+			{
+				collided_primitives.push_back(i);
+			}
+		}
+
+	}
+
+}
+
+
+void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactMeshShapePart * shape0,
+				  btGImpactMeshShapePart * shape1,
+				  const int * pairs, int pair_count)
+{
+	btTriangleShapeEx tri0;
+	btTriangleShapeEx tri1;
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	const int * pair_pointer = pairs;
+
+	while(pair_count--)
+	{
+
+		m_triface0 = *(pair_pointer);
+		m_triface1 = *(pair_pointer+1);
+		pair_pointer+=2;
+
+
+
+		shape0->getBulletTriangle(m_triface0,tri0);
+		shape1->getBulletTriangle(m_triface1,tri1);
+
+
+		//collide two convex shapes
+		if(tri0.overlap_test_conservative(tri1))
+		{
+			convex_vs_convex_collision(body0,body1,&tri0,&tri1);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btGImpactMeshShapePart * shape1,
+					  const int * pairs, int pair_count)
+{
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPrimitiveTriangle ptri0;
+	btPrimitiveTriangle ptri1;
+	BT_TRIANGLE_CONTACT contact_data;
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	const int * pair_pointer = pairs;
+
+	while(pair_count--)
+	{
+
+		m_triface0 = *(pair_pointer);
+		m_triface1 = *(pair_pointer+1);
+		pair_pointer+=2;
+
+
+		shape0->getPrimitiveTriangle(m_triface0,ptri0);
+		shape1->getPrimitiveTriangle(m_triface1,ptri1);
+
+		#ifdef TRI_COLLISION_PROFILING
+		bt_begin_gim02_tri_time();
+		#endif
+
+		ptri0.applyTransform(orgtrans0);
+		ptri1.applyTransform(orgtrans1);
+
+
+		//build planes
+		ptri0.buildTriPlane();
+		ptri1.buildTriPlane();
+		// test conservative
+
+
+
+		if(ptri0.overlap_test_conservative(ptri1))
+		{
+			if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data))
+			{
+
+				int j = contact_data.m_point_count;
+				while(j--)
+				{
+
+					addContactPoint(body0, body1,
+								contact_data.m_points[j],
+								contact_data.m_separating_normal,
+								-contact_data.m_penetration_depth);
+				}
+			}
+		}
+
+		#ifdef TRI_COLLISION_PROFILING
+		bt_end_gim02_tri_time();
+		#endif
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+
+}
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(
+						btCollisionObject * body0,
+					   	btCollisionObject * body1,
+					  	btGImpactShapeInterface * shape0,
+					  	btGImpactShapeInterface * shape1)
+{
+
+	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+		m_part0 = meshshape0->getMeshPartCount();
+
+		while(m_part0--)
+		{
+			gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1);
+		}
+
+		return;
+	}
+
+	if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape1 = static_cast<btGImpactMeshShape *>(shape1);
+		m_part1 = meshshape1->getMeshPartCount();
+
+		while(m_part1--)
+		{
+
+			gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1));
+
+		}
+
+		return;
+	}
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPairSet pairset;
+
+	gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset);
+
+	if(pairset.size()== 0) return;
+
+	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+		shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART)
+	{
+		btGImpactMeshShapePart * shapepart0 = static_cast<btGImpactMeshShapePart * >(shape0);
+		btGImpactMeshShapePart * shapepart1 = static_cast<btGImpactMeshShapePart * >(shape1);
+		//specialized function
+		#ifdef BULLET_TRIANGLE_COLLISION
+		collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+		#else
+		collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size());
+		#endif
+
+		return;
+	}
+
+	//general function
+
+	shape0->lockChildShapes();
+	shape1->lockChildShapes();
+
+	GIM_ShapeRetriever retriever0(shape0);
+	GIM_ShapeRetriever retriever1(shape1);
+
+	bool child_has_transform0 = shape0->childrenHasTransform();
+	bool child_has_transform1 = shape1->childrenHasTransform();
+
+	int i = pairset.size();
+	while(i--)
+	{
+		BT_PAIR * pair = &pairset[i];
+		m_triface0 = pair->m_index1;
+		m_triface1 = pair->m_index2;
+		btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0);
+		btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1);
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0));
+		}
+
+		if(child_has_transform1)
+		{
+			body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1));
+		}
+
+		//collide two convex shapes
+		convex_vs_convex_collision(body0,body1,colshape0,colshape1);
+
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0);
+		}
+
+		if(child_has_transform1)
+		{
+			body1->setWorldTransform(orgtrans1);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+	shape1->unlockChildShapes();
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btCollisionShape * shape1,bool swapped)
+{
+	if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE)
+	{
+		btGImpactMeshShape * meshshape0 = static_cast<btGImpactMeshShape *>(shape0);
+		m_part0 = meshshape0->getMeshPartCount();
+
+		while(m_part0--)
+		{
+
+			gimpact_vs_shape(body0,
+				  body1,
+				  meshshape0->getMeshPart(m_part0),
+				  shape1,swapped);
+
+		}
+
+		return;
+	}
+
+	#ifdef GIMPACT_VS_PLANE_COLLISION
+	if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART &&
+		shape1->getShapeType() == STATIC_PLANE_PROXYTYPE)
+	{
+		btGImpactMeshShapePart * shapepart = static_cast<btGImpactMeshShapePart *>(shape0);
+		btStaticPlaneShape * planeshape = static_cast<btStaticPlaneShape * >(shape1);
+		gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped);
+		return;
+	}
+
+	#endif
+
+
+
+	if(shape1->isCompound())
+	{
+		btCompoundShape * compoundshape = static_cast<btCompoundShape *>(shape1);
+		gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped);
+		return;
+	}
+	else if(shape1->isConcave())
+	{
+		btConcaveShape * concaveshape = static_cast<btConcaveShape *>(shape1);
+		gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped);
+		return;
+	}
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btAlignedObjectArray<int> collided_results;
+
+	gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results);
+
+	if(collided_results.size() == 0) return;
+
+
+	shape0->lockChildShapes();
+
+	GIM_ShapeRetriever retriever0(shape0);
+
+
+	bool child_has_transform0 = shape0->childrenHasTransform();
+
+
+	int i = collided_results.size();
+
+	while(i--)
+	{
+		int child_index = collided_results[i];
+        if(swapped)
+    		m_triface1 = child_index;
+        else
+            m_triface0 = child_index;
+
+		btCollisionShape * colshape0 = retriever0.getChildShape(child_index);
+
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index));
+		}
+
+		//collide two shapes
+		if(swapped)
+		{
+			shape_vs_shape_collision(body1,body0,shape1,colshape0);
+		}
+		else
+		{
+			shape_vs_shape_collision(body0,body1,colshape0,shape1);
+		}
+
+		//restore transforms
+		if(child_has_transform0)
+		{
+			body0->setWorldTransform(orgtrans0);
+		}
+
+	}
+
+	shape0->unlockChildShapes();
+
+}
+
+void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btCompoundShape * shape1,bool swapped)
+{
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	int i = shape1->getNumChildShapes();
+	while(i--)
+	{
+
+		btCollisionShape * colshape1 = shape1->getChildShape(i);
+		btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i);
+
+		body1->setWorldTransform(childtrans1);
+
+		//collide child shape
+		gimpact_vs_shape(body0, body1,
+					  shape0,colshape1,swapped);
+
+
+		//restore transforms
+		body1->setWorldTransform(orgtrans1);
+	}
+}
+
+void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btStaticPlaneShape * shape1,bool swapped)
+{
+
+
+	btTransform orgtrans0 = body0->getWorldTransform();
+	btTransform orgtrans1 = body1->getWorldTransform();
+
+	btPlaneShape * planeshape = static_cast<btPlaneShape *>(shape1);
+	btVector4 plane;
+	planeshape->get_plane_equation_transformed(orgtrans1,plane);
+
+	//test box against plane
+
+	btAABB tribox;
+	shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max);
+	tribox.increment_margin(planeshape->getMargin());
+
+	if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return;
+
+	shape0->lockChildShapes();
+
+	btScalar margin = shape0->getMargin() + planeshape->getMargin();
+
+	btVector3 vertex;
+	int vi = shape0->getVertexCount();
+	while(vi--)
+	{
+		shape0->getVertex(vi,vertex);
+		vertex = orgtrans0(vertex);
+
+		btScalar distance = vertex.dot(plane) - plane[3] - margin;
+
+		if(distance<0.0)//add contact
+		{
+			if(swapped)
+			{
+				addContactPoint(body1, body0,
+					vertex,
+					-plane,
+					distance);
+			}
+			else
+			{
+				addContactPoint(body0, body1,
+					vertex,
+					plane,
+					distance);
+			}
+		}
+	}
+
+	shape0->unlockChildShapes();
+}
+
+
+
+
+class btGImpactTriangleCallback: public btTriangleCallback
+{
+public:
+	btGImpactCollisionAlgorithm * algorithm;
+	btCollisionObject * body0;
+	btCollisionObject * body1;
+	btGImpactShapeInterface * gimpactshape0;
+	bool swapped;
+	btScalar margin;
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]);
+		tri1.setMargin(margin);
+        if(swapped)
+        {
+            algorithm->setPart0(partId);
+            algorithm->setFace0(triangleIndex);
+        }
+        else
+        {
+            algorithm->setPart1(partId);
+            algorithm->setFace1(triangleIndex);
+        }
+		algorithm->gimpact_vs_shape(
+							body0,body1,gimpactshape0,&tri1,swapped);
+	}
+};
+
+
+
+
+void btGImpactCollisionAlgorithm::gimpact_vs_concave(
+				  btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactShapeInterface * shape0,
+				  btConcaveShape * shape1,bool swapped)
+{
+	//create the callback
+	btGImpactTriangleCallback tricallback;
+	tricallback.algorithm = this;
+	tricallback.body0 = body0;
+	tricallback.body1 = body1;
+	tricallback.gimpactshape0 = shape0;
+	tricallback.swapped = swapped;
+	tricallback.margin = shape1->getMargin();
+
+	//getting the trimesh AABB
+	btTransform gimpactInConcaveSpace;
+
+	gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform();
+
+	btVector3 minAABB,maxAABB;
+	shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB);
+
+	shape1->processAllTriangles(&tricallback,minAABB,maxAABB);
+
+}
+
+
+
+void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+    clearCache();
+
+    m_resultOut = resultOut;
+	m_dispatchInfo = &dispatchInfo;
+    btGImpactShapeInterface * gimpactshape0;
+    btGImpactShapeInterface * gimpactshape1;
+
+	if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+	{
+		gimpactshape0 = static_cast<btGImpactShapeInterface *>(body0->getCollisionShape());
+
+		if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+		{
+			gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+			gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1);
+		}
+		else
+		{
+			gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false);
+		}
+
+	}
+	else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE )
+	{
+		gimpactshape1 = static_cast<btGImpactShapeInterface *>(body1->getCollisionShape());
+
+		gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true);
+	}
+}
+
+
+btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	return 1.f;
+
+}
+
+///////////////////////////////////// REGISTERING ALGORITHM //////////////////////////////////////////////
+
+btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf;
+
+//! Use this function for register the algorithm externally
+void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher)
+{
+
+	int i;
+
+	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	{
+		dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf);
+	}
+
+	for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ )
+	{
+		dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf);
+	}
+
+}
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,306 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H
+#define BVH_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btGImpactShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+
+//! Collision Algorithm for GImpact Shapes
+/*!
+For register this algorithm in Bullet, proceed as following:
+ \code
+btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher());
+btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
+ \endcode
+*/
+class btGImpactCollisionAlgorithm : public btCollisionAlgorithm
+{
+protected:
+	btCollisionAlgorithm * m_convex_algorithm;
+    btPersistentManifold * m_manifoldPtr;
+	btManifoldResult* m_resultOut;
+	const btDispatcherInfo * m_dispatchInfo;
+	int m_triface0;
+	int m_part0;
+	int m_triface1;
+	int m_part1;
+
+
+	//! Creates a new contact point
+	SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);		
+		return m_manifoldPtr;
+	}
+
+	SIMD_FORCE_INLINE void destroyConvexAlgorithm()
+	{
+		if(m_convex_algorithm)
+		{
+			m_convex_algorithm->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
+			m_convex_algorithm = NULL;
+		}
+	}
+
+	SIMD_FORCE_INLINE void destroyContactManifolds()
+	{
+		if(m_manifoldPtr == NULL) return;
+		m_dispatcher->releaseManifold(m_manifoldPtr);
+		m_manifoldPtr = NULL;
+	}
+
+	SIMD_FORCE_INLINE void clearCache()
+	{
+		destroyContactManifolds();
+		destroyConvexAlgorithm();
+
+		m_triface0 = -1;
+		m_part0 = -1;
+		m_triface1 = -1;
+		m_part1 = -1;
+	}
+
+	SIMD_FORCE_INLINE btPersistentManifold* getLastManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+
+	// Call before process collision
+	SIMD_FORCE_INLINE void checkManifold(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		if(getLastManifold() == 0)
+		{
+			newContactManifold(body0,body1);
+		}
+
+		m_resultOut->setPersistentManifold(getLastManifold());
+	}
+
+	// Call before process collision
+	SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		checkManifold(body0,body1);
+
+		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
+				body0,body1,getLastManifold());
+		return convex_algorithm ;
+	}
+
+	// Call before process collision
+	SIMD_FORCE_INLINE void checkConvexAlgorithm(btCollisionObject* body0,btCollisionObject* body1)
+	{
+		if(m_convex_algorithm) return;
+		m_convex_algorithm = newAlgorithm(body0,body1);
+	}
+
+
+
+
+	void addContactPoint(btCollisionObject * body0,
+					btCollisionObject * body1,
+					const btVector3 & point,
+					const btVector3 & normal,
+					btScalar distance);
+
+//! Collision routines
+//!@{
+
+	void collide_gjk_triangles(btCollisionObject * body0,
+				  btCollisionObject * body1,
+				  btGImpactMeshShapePart * shape0,
+				  btGImpactMeshShapePart * shape1,
+				  const int * pairs, int pair_count);
+
+	void collide_sat_triangles(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btGImpactMeshShapePart * shape1,
+					  const int * pairs, int pair_count);
+
+
+
+
+	void shape_vs_shape_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1);
+
+	void convex_vs_convex_collision(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btCollisionShape * shape0,
+					  btCollisionShape * shape1);
+
+
+
+	void gimpact_vs_gimpact_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1,btPairSet & pairset);
+
+	void gimpact_vs_shape_find_pairs(
+					  const btTransform & trans0,
+					  const btTransform & trans1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,
+					  btAlignedObjectArray<int> & collided_primitives);
+
+
+	void gimpacttrimeshpart_vs_plane_collision(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactMeshShapePart * shape0,
+					  btStaticPlaneShape * shape1,bool swapped);
+
+
+public:
+
+	btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btGImpactCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
+			return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1);
+		}
+	};
+
+	//! Use this function for register the algorithm externally
+	static void registerAlgorithm(btCollisionDispatcher * dispatcher);
+
+	//! Gets the average time in miliseconds of tree collisions
+	static float getAverageTreeCollisionTime();
+
+	//! Gets the average time in miliseconds of triangle collisions
+	static float getAverageTriangleCollisionTime();
+
+
+	//! Collides two gimpact shapes
+	/*!
+	\pre shape0 and shape1 couldn't be btGImpactMeshShape objects
+	*/
+
+
+	void gimpact_vs_gimpact(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btGImpactShapeInterface * shape1);
+
+	void gimpact_vs_shape(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btCollisionShape * shape1,bool swapped);
+
+	void gimpact_vs_compoundshape(btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btCompoundShape * shape1,bool swapped);
+
+	void gimpact_vs_concave(
+					  btCollisionObject * body0,
+					  btCollisionObject * body1,
+					  btGImpactShapeInterface * shape0,
+					  btConcaveShape * shape1,bool swapped);
+
+
+
+
+		/// Accessor/Mutator pairs for Part and triangleID
+    void 	setFace0(int value) 
+    { 
+    	m_triface0 = value; 
+    }
+    int getFace0() 
+    { 
+    	return m_triface0; 
+    }
+    void setFace1(int value) 
+    { 
+    	m_triface1 = value; 
+    }
+    int getFace1() 
+    { 
+    	return m_triface1; 
+    }
+    void setPart0(int value) 
+    { 
+    	m_part0 = value; 
+    }
+    int getPart0() 
+    { 
+    	return m_part0; 
+    }
+    void setPart1(int value) 
+    { 
+    	m_part1 = value; 
+		}
+    int getPart1() 
+    { 
+    	return m_part1; 
+    }
+
+};
+
+
+//algorithm details
+//#define BULLET_TRIANGLE_COLLISION 1
+#define GIMPACT_VS_PLANE_COLLISION 1
+
+
+
+#endif //BVH_CONCAVE_COLLISION_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h	(revision 1963)
@@ -0,0 +1,60 @@
+/*! \file btGImpactMassUtil.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GIMPACT_MASS_UTIL_H
+#define GIMPACT_MASS_UTIL_H
+
+#include "LinearMath/btTransform.h"
+
+
+
+SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
+	const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
+{
+	btMatrix3x3  rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
+
+	btScalar x2 = transform.getOrigin()[0];
+	x2*= x2;
+	btScalar y2 = transform.getOrigin()[1];
+	y2*= y2;
+	btScalar z2 = transform.getOrigin()[2];
+	z2*= z2;
+
+	btScalar ix = rotatedTensor[0][0]*(y2+z2);
+	btScalar iy = rotatedTensor[1][1]*(x2+z2);
+	btScalar iz = rotatedTensor[2][2]*(x2+y2);
+
+	return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
+}
+
+SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
+{
+	btScalar x2 = point[0]*point[0];
+	btScalar y2 = point[1]*point[1];
+	btScalar z2 = point[2]*point[2];
+	return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
+}
+
+
+#endif //GIMPACT_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp	(revision 1963)
@@ -0,0 +1,529 @@
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGImpactQuantizedBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+btClock g_q_tree_clock;
+
+
+float g_q_accum_tree_collision_time = 0;
+int g_q_count_traversing = 0;
+
+
+void bt_begin_gim02_q_tree_time()
+{
+	g_q_tree_clock.reset();
+}
+
+void bt_end_gim02_q_tree_time()
+{
+	g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
+	g_q_count_traversing++;
+}
+
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
+{
+	if(g_q_count_traversing == 0) return 0;
+
+	float avgtime = g_q_accum_tree_collision_time;
+	avgtime /= (float)g_q_count_traversing;
+
+	g_q_accum_tree_collision_time = 0;
+	g_q_count_traversing = 0;
+	return avgtime;
+
+//	float avgtime = g_q_count_traversing;
+//	g_q_count_traversing = 0;
+//	return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btQuantizedBvhTree /////////////////////////////////
+
+void btQuantizedBvhTree::calc_quantization(
+	BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+{
+	//calc globa box
+	btAABB global_bound;
+	global_bound.invalidate();
+
+	for (int i=0;i<primitive_boxes.size() ;i++ )
+	{
+		global_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	bt_calc_quantization_parameters(
+		m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
+
+}
+
+
+
+int btQuantizedBvhTree::_calc_splitting_axis(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+
+	int i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	int numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+int btQuantizedBvhTree::_sort_and_calc_splitting_index(
+	BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	int endIndex, int splitAxis)
+{
+	int i;
+	int splitIndex =startIndex;
+	int numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	splitValue = means[splitAxis];
+
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		if (center[splitAxis] > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			//swapLeafNodes(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	int rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+
+}
+
+
+void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+{
+	int curIndex = m_num_nodes;
+	m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if ((endIndex-startIndex)==1)
+	{
+	    //We have a leaf node
+	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+		return;
+	}
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,
+			splitIndex//split axis
+			);
+
+
+	//calc this node bounding box
+
+	btAABB node_bound;
+	node_bound.invalidate();
+
+	for (int i=startIndex;i<endIndex;i++)
+	{
+		node_bound.merge(primitive_boxes[i].m_bound);
+	}
+
+	setNodeBound(curIndex,node_bound);
+
+
+	//build left branch
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+	//build right branch
+	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btQuantizedBvhTree::build_tree(
+	BT_BVH_DATA_ARRAY & primitive_boxes)
+{
+	calc_quantization(primitive_boxes);
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactQuantizedBvh
+
+void btGImpactQuantizedBvh::refit()
+{
+	int nodecount = getNodeCount();
+	while(nodecount--)
+	{
+		if(isLeafNode(nodecount))
+		{
+			btAABB leafbox;
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+			setNodeBound(nodecount,leafbox);
+		}
+		else
+		{
+			//const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+			//get left bound
+			btAABB bound;
+			bound.invalidate();
+
+			btAABB temp_box;
+
+			int child_node = getLeftNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			child_node = getRightNode(nodecount);
+			if(child_node)
+			{
+				getNodeBound(child_node,temp_box);
+				bound.merge(temp_box);
+			}
+
+			setNodeBound(nodecount,bound);
+		}
+	}
+}
+
+//! this rebuild the entire set
+void btGImpactQuantizedBvh::buildSet()
+{
+	//obtain primitive boxes
+	BT_BVH_DATA_ARRAY primitive_boxes;
+	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	{
+		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+		 primitive_boxes[i].m_data = i;
+	}
+
+	m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	//quantize box
+
+	unsigned short quantizedMin[3];
+	unsigned short quantizedMax[3];
+
+	m_box_tree.quantizePoint(quantizedMin,box.m_min);
+	m_box_tree.quantizePoint(quantizedMax,box.m_max);
+
+
+	while (curIndex < numNodes)
+	{
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData(curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::rayQuery(
+	const btVector3 & ray_dir,const btVector3 & ray_origin ,
+	btAlignedObjectArray<int> & collided_results) const
+{
+	int curIndex = 0;
+	int numNodes = getNodeCount();
+
+	while (curIndex < numNodes)
+	{
+		btAABB bound;
+		getNodeBound(curIndex,bound);
+
+		//catch bugs in tree data
+
+		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool isleafnode = isLeafNode(curIndex);
+
+		if (isleafnode && aabbOverlap)
+		{
+			collided_results.push_back(getNodeData( curIndex));
+		}
+
+		if (aabbOverlap || isleafnode)
+		{
+			//next subnode
+			curIndex++;
+		}
+		else
+		{
+			//skip node
+			curIndex+= getEscapeNodeIndex(curIndex);
+		}
+	}
+	if(collided_results.size()>0) return true;
+	return false;
+}
+
+
+SIMD_FORCE_INLINE bool _quantized_node_collision(
+	btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0 ,int node1, bool complete_primitive_tests)
+{
+	btAABB box0;
+	boxset0->getNodeBound(node0,box0);
+	btAABB box1;
+	boxset1->getNodeBound(node1,box1);
+
+	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+//	box1.appy_transform_trans_cache(trans_cache_1to0);
+//	return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_quantized_collision_pairs_recursive(
+	btGImpactQuantizedBvh * boxset0, btGImpactQuantizedBvh * boxset1,
+	btPairSet * collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+	if( _quantized_node_collision(
+		boxset0,boxset1,trans_cache_1to0,
+		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+	if(boxset0->isLeafNode(node0))
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+			// collision result
+			collision_pairs->push_pair(
+				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+			return;
+		}
+		else
+		{
+
+			//collide left recursive
+
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getLeftNode(node1),false);
+
+			//collide right recursive
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								node0,boxset1->getRightNode(node1),false);
+
+
+		}
+	}
+	else
+	{
+		if(boxset1->isLeafNode(node1))
+		{
+
+			//collide left recursive
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getLeftNode(node0),node1,false);
+
+
+			//collide right recursive
+
+			_find_quantized_collision_pairs_recursive(
+								boxset0,boxset1,
+								collision_pairs,trans_cache_1to0,
+								boxset0->getRightNode(node0),node1,false);
+
+
+		}
+		else
+		{
+			//collide left0 left1
+
+
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide left0 right1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+			//collide right0 left1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+			//collide right0 right1
+
+			_find_quantized_collision_pairs_recursive(
+				boxset0,boxset1,
+				collision_pairs,trans_cache_1to0,
+				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+		}// else if node1 is not a leaf
+	}// else if node0 is not a leaf
+}
+
+
+void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
+		btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+		btPairSet & collision_pairs)
+{
+
+	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+	bt_begin_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+	_find_quantized_collision_pairs_recursive(
+		boxset0,boxset1,
+		&collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+	bt_end_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h	(revision 1963)
@@ -0,0 +1,379 @@
+#ifndef GIM_QUANTIZED_SET_H_INCLUDED
+#define GIM_QUANTIZED_SET_H_INCLUDED
+
+/*! \file btGImpactQuantizedBvh.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGImpactBvh.h"
+#include "btQuantization.h"
+
+
+
+/*! \defgroup BOX_TREES
+
+
+
+*/
+//! @{
+
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrDataIndex;
+
+	BT_QUANTIZED_BVH_NODE()
+	{
+		m_escapeIndexOrDataIndex = 0;
+	}
+
+	SIMD_FORCE_INLINE bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrDataIndex>=0);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex < 0);
+		return -m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setEscapeIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = -index;
+	}
+
+	SIMD_FORCE_INLINE int getDataIndex() const
+	{
+		//btAssert(m_escapeIndexOrDataIndex >= 0);
+
+		return m_escapeIndexOrDataIndex;
+	}
+
+	SIMD_FORCE_INLINE void setDataIndex(int index)
+	{
+		m_escapeIndexOrDataIndex = index;
+	}
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+	{
+		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
+		   m_quantizedAabbMax[0] < quantizedMin[0] ||
+		   m_quantizedAabbMin[1] > quantizedMax[1] ||
+		   m_quantizedAabbMax[1] < quantizedMin[1] ||
+		   m_quantizedAabbMin[2] > quantizedMax[2] ||
+		   m_quantizedAabbMax[2] < quantizedMin[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+};
+
+
+
+class BT_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
+{
+};
+
+
+
+
+//! Basic Box tree structure
+class btQuantizedBvhTree
+{
+protected:
+	int m_num_nodes;
+	BT_QUANTIZED_BVH_NODE_ARRAY m_node_array;
+	btAABB m_global_bound;
+	btVector3 m_bvhQuantization;
+protected:
+	void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );
+
+	int _sort_and_calc_splitting_index(
+		BT_BVH_DATA_ARRAY & primitive_boxes,
+		 int startIndex,  int endIndex, int splitAxis);
+
+	int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+
+	void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
+public:
+	btQuantizedBvhTree()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes);
+
+	SIMD_FORCE_INLINE void quantizePoint(
+		unsigned short * quantizedpoint, const btVector3 & point) const
+	{
+		bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
+	}
+
+
+	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
+		int node_index,
+		unsigned short * quantizedMin,unsigned short * quantizedMax) const
+	{
+		return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
+	}
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_node_array[nodeindex].isLeafNode();
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getDataIndex();
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
+	{
+		bound.m_min = bt_unquantize(
+			m_node_array[nodeindex].m_quantizedAabbMin,
+			m_global_bound.m_min,m_bvhQuantization);
+
+		bound.m_max = bt_unquantize(
+			m_node_array[nodeindex].m_quantizedAabbMax,
+			m_global_bound.m_min,m_bvhQuantization);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMin,
+							bound.m_min,
+							m_global_bound.m_min,
+							m_global_bound.m_max,
+							m_bvhQuantization);
+
+		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMax,
+							bound.m_max,
+							m_global_bound.m_min,
+							m_global_bound.m_max,
+							m_bvhQuantization);
+	}
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return nodeindex+1;
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
+		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_node_array[nodeindex].getEscapeIndex();
+	}
+
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	{
+		return &m_node_array[index];
+	}
+
+	//!@}
+};
+
+
+
+//! Structure for containing Boxes
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like btPrimitiveManagerBase )
+*/
+class btGImpactQuantizedBvh
+{
+protected:
+	btQuantizedBvhTree m_box_tree;
+	btPrimitiveManagerBase * m_primitive_manager;
+
+protected:
+	//stackless refit
+	void refit();
+public:
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactQuantizedBvh()
+	{
+		m_primitive_manager = NULL;
+	}
+
+	//! this constructor doesn't build the tree. you must call	buildSet
+	btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btAABB getGlobalBox()  const
+	{
+		btAABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	void buildSet();
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
+		 const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
+	{
+		btAABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		btAlignedObjectArray<int> & collided_results) const;
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager->is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE int getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+
+	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
+	{
+		return m_box_tree.getLeftNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
+	{
+		return m_box_tree.getRightNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
+	{
+		return m_box_tree.getEscapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
+	{
+		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+
+	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
+	{
+		return m_box_tree.get_node_pointer(index);
+	}
+
+//! @}
+
+	static float getAverageTreeCollisionTime();
+
+
+	static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+		btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
+		btPairSet & collision_pairs);
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.cpp	(revision 1963)
@@ -0,0 +1,183 @@
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btGImpactShape.h"
+#include "btGImpactMassUtil.h"
+
+
+#define CALC_EXACT_INERTIA 1
+
+void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	lockChildShapes();
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getNumChildShapes();
+	btScalar shapemass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 temp_inertia;
+		m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
+		if(childrenHasTransform())
+		{
+			inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
+		}
+		else
+		{
+			inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
+		}
+
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+	unlockChildShapes();
+}
+
+
+
+void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+	lockChildShapes();
+
+
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getVertexCount();
+	btScalar pointmass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 pointintertia;
+		this->getVertex(i,pointintertia);
+		pointintertia = gim_get_point_inertia(pointintertia,pointmass);
+		inertia+=pointintertia;
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+
+	unlockChildShapes();
+}
+
+void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+
+#ifdef CALC_EXACT_INERTIA
+	inertia.setValue(0.f,0.f,0.f);
+
+	int i = this->getMeshPartCount();
+	btScalar partmass = mass/btScalar(i);
+
+	while(i--)
+	{
+		btVector3 partinertia;
+		getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
+		inertia+=partinertia;
+	}
+
+#else
+
+	// Calc box inertia
+
+	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
+	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
+	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
+	const btScalar x2 = lx*lx;
+	const btScalar y2 = ly*ly;
+	const btScalar z2 = lz*lz;
+	const btScalar scaledmass = mass * btScalar(0.08333333);
+
+	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
+
+#endif
+}
+
+void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const
+{
+}
+
+
+void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	lockChildShapes();
+	btAABB box;
+	box.m_min = aabbMin;
+	box.m_max = aabbMax;
+
+	btAlignedObjectArray<int> collided;
+	m_box_set.boxQuery(box,collided);
+
+	if(collided.size()==0)
+	{
+		unlockChildShapes();
+		return;
+	}
+
+	int part = (int)getPart();
+	btPrimitiveTriangle triangle;
+	int i = collided.size();
+	while(i--)
+	{
+		this->getPrimitiveTriangle(collided[i],triangle);
+		callback->processTriangle(triangle.m_vertices,part,collided[i]);
+	}
+	unlockChildShapes();
+
+}
+
+void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+	int i = m_mesh_parts.size();
+	while(i--)
+	{
+		m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
+	}
+}
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGImpactShape.h	(revision 1963)
@@ -0,0 +1,1110 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GIMPACT_SHAPE_H
+#define GIMPACT_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btGImpactQuantizedBvh.h" // box tree class
+
+
+//! declare Quantized trees, (you can change to float based trees)
+typedef btGImpactQuantizedBvh btGImpactBoxSet;
+
+enum eGIMPACT_SHAPE_TYPE
+{
+	CONST_GIMPACT_COMPOUND_SHAPE = 0,
+	CONST_GIMPACT_TRIMESH_SHAPE_PART,
+	CONST_GIMPACT_TRIMESH_SHAPE
+};
+
+
+//! Helper class for tetrahedrons
+class btTetrahedronShapeEx:public btBU_Simplex1to4
+{
+public:
+	btTetrahedronShapeEx()
+	{
+		m_numVertices = 4;
+	}
+
+
+	SIMD_FORCE_INLINE void setVertices(
+		const btVector3 & v0,const btVector3 & v1,
+		const btVector3 & v2,const btVector3 & v3)
+	{
+		m_vertices[0] = v0;
+		m_vertices[1] = v1;
+		m_vertices[2] = v2;
+		m_vertices[3] = v3;
+		recalcLocalAabb();
+	}
+};
+
+
+//! Base class for gimpact shapes
+class btGImpactShapeInterface : public btConcaveShape
+{
+protected:
+    btAABB m_localAABB;
+    bool m_needs_update;
+    btVector3  localScaling;
+    btGImpactBoxSet m_box_set;// optionally boxset
+
+	//! use this function for perfofm refit in bounding boxes
+    //! use this function for perfofm refit in bounding boxes
+    virtual void calcLocalAABB()
+    {
+		lockChildShapes();
+    	if(m_box_set.getNodeCount() == 0)
+    	{
+    		m_box_set.buildSet();
+    	}
+    	else
+    	{
+    		m_box_set.update();
+    	}
+    	unlockChildShapes();
+
+    	m_localAABB = m_box_set.getGlobalBox();
+    }
+
+
+public:
+	btGImpactShapeInterface()
+	{
+		m_shapeType=GIMPACT_SHAPE_PROXYTYPE;
+		m_localAABB.invalidate();
+		m_needs_update = true;
+		localScaling.setValue(1.f,1.f,1.f);
+	}
+
+
+	//! performs refit operation
+	/*!
+	Updates the entire Box set of this shape.
+	\pre postUpdate() must be called for attemps to calculating the box set, else this function
+		will does nothing.
+	\post if m_needs_update == true, then it calls calcLocalAABB();
+	*/
+    SIMD_FORCE_INLINE void updateBound()
+    {
+    	if(!m_needs_update) return;
+    	calcLocalAABB();
+    	m_needs_update  = false;
+    }
+
+    //! If the Bounding box is not updated, then this class attemps to calculate it.
+    /*!
+    \post Calls updateBound() for update the box set.
+    */
+    void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+    {
+        btAABB transformedbox = m_localAABB;
+        transformedbox.appy_transform(t);
+        aabbMin = transformedbox.m_min;
+        aabbMax = transformedbox.m_max;
+    }
+
+    //! Tells to this object that is needed to refit the box set
+    virtual void postUpdate()
+    {
+    	m_needs_update = true;
+    }
+
+	//! Obtains the local box, which is the global calculated box of the total of subshapes
+	SIMD_FORCE_INLINE const btAABB & getLocalBox()
+	{
+		return m_localAABB;
+	}
+
+
+    virtual int	getShapeType() const
+    {
+        return GIMPACT_SHAPE_PROXYTYPE;
+    }
+
+    /*!
+	\post You must call updateBound() for update the box set.
+	*/
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		localScaling = scaling;
+		postUpdate();
+	}
+
+	virtual const btVector3& getLocalScaling() const
+	{
+		return localScaling;
+	}
+
+
+	virtual void setMargin(btScalar margin)
+    {
+    	m_collisionMargin = margin;
+    	int i = getNumChildShapes();
+    	while(i--)
+    	{
+			btCollisionShape* child = getChildShape(i);
+			child->setMargin(margin);
+    	}
+
+		m_needs_update = true;
+    }
+
+
+	//! Subshape member functions
+	//!@{
+
+	//! Base method for determinig which kind of GIMPACT shape we get
+	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() = 0;
+
+	//! gets boxset
+	SIMD_FORCE_INLINE btGImpactBoxSet * getBoxSet()
+	{
+		return &m_box_set;
+	}
+
+	//! Determines if this class has a hierarchy structure for sorting its primitives
+	SIMD_FORCE_INLINE bool hasBoxSet()  const
+	{
+		if(m_box_set.getNodeCount() == 0) return false;
+		return true;
+	}
+
+	//! Obtains the primitive manager
+	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const = 0;
+
+
+	//! Gets the number of children
+	virtual int	getNumChildShapes() const  = 0;
+
+	//! if true, then its children must get transforms.
+	virtual bool childrenHasTransform() const = 0;
+
+	//! Determines if this shape has triangles
+	virtual bool needsRetrieveTriangles() const = 0;
+
+	//! Determines if this shape has tetrahedrons
+	virtual bool needsRetrieveTetrahedrons() const = 0;
+
+	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0;
+
+	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0;
+
+
+
+	//! call when reading child shapes
+	virtual void lockChildShapes() const
+	{
+	}
+
+	virtual void unlockChildShapes() const
+	{
+	}
+
+	//! if this trimesh
+	SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const
+	{
+		getPrimitiveManager()->get_primitive_triangle(index,triangle);
+	}
+
+
+	//! Retrieves the bound from a child
+    /*!
+    */
+    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+    {
+        btAABB child_aabb;
+        getPrimitiveManager()->get_primitive_box(child_index,child_aabb);
+        child_aabb.appy_transform(t);
+        aabbMin = child_aabb.m_min;
+        aabbMax = child_aabb.m_max;
+    }
+
+	//! Gets the children
+	virtual btCollisionShape* getChildShape(int index) = 0;
+
+
+	//! Gets the child
+	virtual const btCollisionShape* getChildShape(int index) const = 0;
+
+	//! Gets the children transform
+	virtual btTransform	getChildTransform(int index) const = 0;
+
+	//! Sets the children transform
+	/*!
+	\post You must call updateBound() for update the box set.
+	*/
+	virtual void setChildTransform(int index, const btTransform & transform) = 0;
+
+	//!@}
+
+
+	//! virtual method for ray collision
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback)  const
+	{
+	}
+
+	//! Function for retrieve triangles.
+	/*!
+	It gives the triangles in local space
+	*/
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+	{
+	}
+
+	//!@}
+
+};
+
+
+//! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once
+/*!
+This class only can manage Convex subshapes
+*/
+class btGImpactCompoundShape	: public btGImpactShapeInterface
+{
+public:
+	//! compound primitive manager
+	class CompoundPrimitiveManager:public btPrimitiveManagerBase
+	{
+	public:
+		btGImpactCompoundShape * m_compoundShape;
+
+
+		CompoundPrimitiveManager(const CompoundPrimitiveManager& compound)
+		{
+			m_compoundShape = compound.m_compoundShape;
+		}
+
+		CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape)
+		{
+			m_compoundShape = compoundShape;
+		}
+
+		CompoundPrimitiveManager()
+		{
+			m_compoundShape = NULL;
+		}
+
+		virtual bool is_trimesh() const
+		{
+			return false;
+		}
+
+		virtual int get_primitive_count() const
+		{
+			return (int )m_compoundShape->getNumChildShapes();
+		}
+
+		virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+		{
+			btTransform prim_trans;
+			if(m_compoundShape->childrenHasTransform())
+			{
+				prim_trans = m_compoundShape->getChildTransform(prim_index);
+			}
+			else
+			{
+				prim_trans.setIdentity();
+			}
+			const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index);
+			shape->getAabb(prim_trans,primbox.m_min,primbox.m_max);
+		}
+
+		virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+		{
+			btAssert(0);
+		}
+
+	};
+
+
+
+protected:
+	CompoundPrimitiveManager m_primitive_manager;
+	btAlignedObjectArray<btTransform>		m_childTransforms;
+	btAlignedObjectArray<btCollisionShape*>	m_childShapes;
+
+
+public:
+
+	btGImpactCompoundShape(bool children_has_transform = true)
+	{
+		m_primitive_manager.m_compoundShape = this;
+		m_box_set.setPrimitiveManager(&m_primitive_manager);
+	}
+
+	virtual ~btGImpactCompoundShape()
+	{
+	}
+
+
+	//! if true, then its children must get transforms.
+	virtual bool childrenHasTransform() const
+	{
+		if(m_childTransforms.size()==0) return false;
+		return true;
+	}
+
+
+	//! Obtains the primitive manager
+	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	{
+		return &m_primitive_manager;
+	}
+
+	//! Obtains the compopund primitive manager
+	SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager()
+	{
+		return &m_primitive_manager;
+	}
+
+	//! Gets the number of children
+	virtual int	getNumChildShapes() const
+	{
+		return m_childShapes.size();
+	}
+
+
+	//! Use this method for adding children. Only Convex shapes are allowed.
+	void addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+	{
+		btAssert(shape->isConvex());
+		m_childTransforms.push_back(localTransform);
+		m_childShapes.push_back(shape);
+	}
+
+	//! Use this method for adding children. Only Convex shapes are allowed.
+	void addChildShape(btCollisionShape* shape)
+	{
+		btAssert(shape->isConvex());
+		m_childShapes.push_back(shape);
+	}
+
+	//! Gets the children
+	virtual btCollisionShape* getChildShape(int index)
+	{
+		return m_childShapes[index];
+	}
+
+	//! Gets the children
+	virtual const btCollisionShape* getChildShape(int index) const
+	{
+		return m_childShapes[index];
+	}
+
+	//! Retrieves the bound from a child
+    /*!
+    */
+    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+    {
+
+    	if(childrenHasTransform())
+    	{
+    		m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax);
+    	}
+    	else
+    	{
+    		m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax);
+    	}
+    }
+
+
+	//! Gets the children transform
+	virtual btTransform	getChildTransform(int index) const
+	{
+		btAssert(m_childTransforms.size() == m_childShapes.size());
+		return m_childTransforms[index];
+	}
+
+	//! Sets the children transform
+	/*!
+	\post You must call updateBound() for update the box set.
+	*/
+	virtual void setChildTransform(int index, const btTransform & transform)
+	{
+		btAssert(m_childTransforms.size() == m_childShapes.size());
+		m_childTransforms[index] = transform;
+		postUpdate();
+	}
+
+	//! Determines if this shape has triangles
+	virtual bool needsRetrieveTriangles() const
+	{
+		return false;
+	}
+
+	//! Determines if this shape has tetrahedrons
+	virtual bool needsRetrieveTetrahedrons() const
+	{
+		return false;
+	}
+
+
+	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	{
+		btAssert(0);
+	}
+
+	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	{
+		btAssert(0);
+	}
+
+
+	//! Calculates the exact inertia tensor for this shape
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+	virtual const char*	getName()const
+	{
+		return "GImpactCompound";
+	}
+
+	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+	{
+		return CONST_GIMPACT_COMPOUND_SHAPE;
+	}
+
+};
+
+
+
+//! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface.
+/*!
+- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh
+- When making operations with this shape, you must call <b>lock</b> before accessing to the trimesh primitives, and then call <b>unlock</b>
+- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices.
+
+*/
+class btGImpactMeshShapePart : public btGImpactShapeInterface
+{
+public:
+	//! Trimesh primitive manager
+	/*!
+	Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism
+	*/
+	class TrimeshPrimitiveManager:public btPrimitiveManagerBase
+	{
+	public:
+		btScalar m_margin;
+		btStridingMeshInterface * m_meshInterface;
+		btVector3 m_scale;
+		int m_part;
+		int m_lock_count;
+		const unsigned char *vertexbase;
+		int numverts;
+		PHY_ScalarType type;
+		int stride;
+		const unsigned char *indexbase;
+		int indexstride;
+		int  numfaces;
+		PHY_ScalarType indicestype;
+
+		TrimeshPrimitiveManager()
+		{
+			m_meshInterface = NULL;
+			m_part = 0;
+			m_margin = 0.01f;
+			m_scale = btVector3(1.f,1.f,1.f);
+			m_lock_count = 0;
+			vertexbase = 0;
+			numverts = 0;
+			stride = 0;
+			indexbase = 0;
+			indexstride = 0;
+			numfaces = 0;
+		}
+
+ 		TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager)
+		{
+			m_meshInterface = manager.m_meshInterface;
+			m_part = manager.m_part;
+			m_margin = manager.m_margin;
+			m_scale = manager.m_scale;
+			m_lock_count = 0;
+			vertexbase = 0;
+			numverts = 0;
+			stride = 0;
+			indexbase = 0;
+			indexstride = 0;
+			numfaces = 0;
+
+		}
+
+		TrimeshPrimitiveManager(
+			btStridingMeshInterface * meshInterface,	int part)
+		{
+			m_meshInterface = meshInterface;
+			m_part = part;
+			m_scale = m_meshInterface->getScaling();
+			m_margin = 0.1f;
+			m_lock_count = 0;
+			vertexbase = 0;
+			numverts = 0;
+			stride = 0;
+			indexbase = 0;
+			indexstride = 0;
+			numfaces = 0;
+
+		}
+
+
+		void lock()
+		{
+			if(m_lock_count>0)
+			{
+				m_lock_count++;
+				return;
+			}
+			m_meshInterface->getLockedReadOnlyVertexIndexBase(
+				&vertexbase,numverts,
+				type, stride,&indexbase, indexstride, numfaces,indicestype,m_part);
+
+			m_lock_count = 1;
+		}
+
+		void unlock()
+		{
+			if(m_lock_count == 0) return;
+			if(m_lock_count>1)
+			{
+				--m_lock_count;
+				return;
+			}
+			m_meshInterface->unLockReadOnlyVertexBase(m_part);
+			vertexbase = NULL;
+			m_lock_count = 0;
+		}
+
+		virtual bool is_trimesh() const
+		{
+			return true;
+		}
+
+		virtual int get_primitive_count() const
+		{
+			return (int )numfaces;
+		}
+
+		SIMD_FORCE_INLINE int get_vertex_count() const
+		{
+			return (int )numverts;
+		}
+
+		SIMD_FORCE_INLINE void get_indices(int face_index,int &i0,int &i1,int &i2) const
+		{
+			if(indicestype == PHY_SHORT)
+			{
+				short * s_indices = (short *)(indexbase + face_index*indexstride);
+				i0 = s_indices[0];
+				i1 = s_indices[1];
+				i2 = s_indices[2];
+			}
+			else
+			{
+				int * i_indices = (int *)(indexbase + face_index*indexstride);
+				i0 = i_indices[0];
+				i1 = i_indices[1];
+				i2 = i_indices[2];
+			}
+		}
+
+		SIMD_FORCE_INLINE void get_vertex(int vertex_index, btVector3 & vertex) const
+		{
+			if(type == PHY_DOUBLE)
+			{
+				double * dvertices = (double *)(vertexbase + vertex_index*stride);
+				vertex[0] = btScalar(dvertices[0]*m_scale[0]);
+				vertex[1] = btScalar(dvertices[1]*m_scale[1]);
+				vertex[2] = btScalar(dvertices[2]*m_scale[2]);
+			}
+			else
+			{
+				float * svertices = (float *)(vertexbase + vertex_index*stride);
+				vertex[0] = svertices[0]*m_scale[0];
+				vertex[1] = svertices[1]*m_scale[1];
+				vertex[2] = svertices[2]*m_scale[2];
+			}
+		}
+
+		virtual void get_primitive_box(int prim_index ,btAABB & primbox) const
+		{
+			btPrimitiveTriangle  triangle;
+			get_primitive_triangle(prim_index,triangle);
+			primbox.calc_from_triangle_margin(
+				triangle.m_vertices[0],
+				triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin);
+		}
+
+		virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const
+		{
+			int indices[3];
+			get_indices(prim_index,indices[0],indices[1],indices[2]);
+			get_vertex(indices[0],triangle.m_vertices[0]);
+			get_vertex(indices[1],triangle.m_vertices[1]);
+			get_vertex(indices[2],triangle.m_vertices[2]);
+			triangle.m_margin = m_margin;
+		}
+
+		SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const
+		{
+			int indices[3];
+			get_indices(prim_index,indices[0],indices[1],indices[2]);
+			get_vertex(indices[0],triangle.m_vertices1[0]);
+			get_vertex(indices[1],triangle.m_vertices1[1]);
+			get_vertex(indices[2],triangle.m_vertices1[2]);
+			triangle.setMargin(m_margin);
+		}
+
+	};
+
+
+protected:
+	TrimeshPrimitiveManager m_primitive_manager;
+public:
+
+	btGImpactMeshShapePart()
+	{
+		m_box_set.setPrimitiveManager(&m_primitive_manager);
+	}
+
+
+	btGImpactMeshShapePart(btStridingMeshInterface * meshInterface,	int part)
+	{
+		m_primitive_manager.m_meshInterface = meshInterface;
+		m_primitive_manager.m_part = part;
+		m_box_set.setPrimitiveManager(&m_primitive_manager);
+	}
+
+	virtual ~btGImpactMeshShapePart()
+	{
+	}
+
+	//! if true, then its children must get transforms.
+	virtual bool childrenHasTransform() const
+	{
+		return false;
+	}
+
+
+	//! call when reading child shapes
+	virtual void lockChildShapes() const
+	{
+		void * dummy = (void*)(m_box_set.getPrimitiveManager());
+		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
+		dummymanager->lock();
+	}
+
+	virtual void unlockChildShapes()  const
+	{
+		void * dummy = (void*)(m_box_set.getPrimitiveManager());
+		TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>(dummy);
+		dummymanager->unlock();
+	}
+
+	//! Gets the number of children
+	virtual int	getNumChildShapes() const
+	{
+		return m_primitive_manager.get_primitive_count();
+	}
+
+
+	//! Gets the children
+	virtual btCollisionShape* getChildShape(int index)
+	{
+		btAssert(0);
+		return NULL;
+	}
+
+
+
+	//! Gets the child
+	virtual const btCollisionShape* getChildShape(int index) const
+	{
+		btAssert(0);
+		return NULL;
+	}
+
+	//! Gets the children transform
+	virtual btTransform	getChildTransform(int index) const
+	{
+		btAssert(0);
+		return btTransform();
+	}
+
+	//! Sets the children transform
+	/*!
+	\post You must call updateBound() for update the box set.
+	*/
+	virtual void setChildTransform(int index, const btTransform & transform)
+	{
+		btAssert(0);
+	}
+
+
+	//! Obtains the primitive manager
+	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	{
+		return &m_primitive_manager;
+	}
+
+	SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager()
+	{
+		return &m_primitive_manager;
+	}
+
+
+
+
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+
+
+	virtual const char*	getName()const
+	{
+		return "GImpactMeshShapePart";
+	}
+
+	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+	{
+		return CONST_GIMPACT_TRIMESH_SHAPE_PART;
+	}
+
+	//! Determines if this shape has triangles
+	virtual bool needsRetrieveTriangles() const
+	{
+		return true;
+	}
+
+	//! Determines if this shape has tetrahedrons
+	virtual bool needsRetrieveTetrahedrons() const
+	{
+		return false;
+	}
+
+	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	{
+		m_primitive_manager.get_bullet_triangle(prim_index,triangle);
+	}
+
+	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	{
+		btAssert(0);
+	}
+
+
+
+	SIMD_FORCE_INLINE int getVertexCount() const
+	{
+		return m_primitive_manager.get_vertex_count();
+	}
+
+	SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const
+	{
+		m_primitive_manager.get_vertex(vertex_index,vertex);
+	}
+
+	SIMD_FORCE_INLINE void setMargin(btScalar margin)
+    {
+    	m_primitive_manager.m_margin = margin;
+    	postUpdate();
+    }
+
+    SIMD_FORCE_INLINE btScalar getMargin() const
+    {
+    	return m_primitive_manager.m_margin;
+    }
+
+    virtual void	setLocalScaling(const btVector3& scaling)
+    {
+    	m_primitive_manager.m_scale = scaling;
+    	postUpdate();
+    }
+
+    virtual const btVector3& getLocalScaling() const
+    {
+    	return m_primitive_manager.m_scale;
+    }
+
+    SIMD_FORCE_INLINE int getPart() const
+    {
+    	return (int)m_primitive_manager.m_part;
+    }
+
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+};
+
+
+//! This class manages a mesh supplied by the btStridingMeshInterface interface.
+/*!
+Set of btGImpactMeshShapePart parts
+- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShape, then you must call updateBound() after creating the mesh
+
+- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices.
+
+*/
+class btGImpactMeshShape : public btGImpactShapeInterface
+{
+protected:
+	btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
+	void buildMeshParts(btStridingMeshInterface * meshInterface)
+	{
+		for (int i=0;i<meshInterface->getNumSubParts() ;++i )
+		{
+			btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i);
+			m_mesh_parts.push_back(newpart);
+		}
+	}
+
+	//! use this function for perfofm refit in bounding boxes
+    virtual void calcLocalAABB()
+    {
+    	m_localAABB.invalidate();
+    	int i = m_mesh_parts.size();
+    	while(i--)
+    	{
+    		m_mesh_parts[i]->updateBound();
+    		m_localAABB.merge(m_mesh_parts[i]->getLocalBox());
+    	}
+    }
+
+public:
+	btGImpactMeshShape(btStridingMeshInterface * meshInterface)
+	{
+		buildMeshParts(meshInterface);
+	}
+
+	virtual ~btGImpactMeshShape()
+	{
+		int i = m_mesh_parts.size();
+    	while(i--)
+    	{
+			btGImpactMeshShapePart * part = m_mesh_parts[i];
+			delete part;
+    	}
+		m_mesh_parts.clear();
+	}
+
+
+
+	int getMeshPartCount() const
+	{
+		return m_mesh_parts.size();
+	}
+
+	btGImpactMeshShapePart * getMeshPart(int index)
+	{
+		return m_mesh_parts[index];
+	}
+
+
+
+	const btGImpactMeshShapePart * getMeshPart(int index) const
+	{
+		return m_mesh_parts[index];
+	}
+
+
+	virtual void	setLocalScaling(const btVector3& scaling)
+	{
+		localScaling = scaling;
+
+		int i = m_mesh_parts.size();
+    	while(i--)
+    	{
+			btGImpactMeshShapePart * part = m_mesh_parts[i];
+			part->setLocalScaling(scaling);
+    	}
+
+		m_needs_update = true;
+	}
+
+	virtual void setMargin(btScalar margin)
+    {
+    	m_collisionMargin = margin;
+
+		int i = m_mesh_parts.size();
+    	while(i--)
+    	{
+			btGImpactMeshShapePart * part = m_mesh_parts[i];
+			part->setMargin(margin);
+    	}
+
+		m_needs_update = true;
+    }
+
+	//! Tells to this object that is needed to refit all the meshes
+    virtual void postUpdate()
+    {
+		int i = m_mesh_parts.size();
+    	while(i--)
+    	{
+			btGImpactMeshShapePart * part = m_mesh_parts[i];
+			part->postUpdate();
+    	}
+
+    	m_needs_update = true;
+    }
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+
+	//! Obtains the primitive manager
+	virtual const btPrimitiveManagerBase * getPrimitiveManager()  const
+	{
+		btAssert(0);
+		return NULL;
+	}
+
+
+	//! Gets the number of children
+	virtual int	getNumChildShapes() const
+	{
+		btAssert(0);
+		return 0;
+	}
+
+
+	//! if true, then its children must get transforms.
+	virtual bool childrenHasTransform() const
+	{
+		btAssert(0);
+		return false;
+	}
+
+	//! Determines if this shape has triangles
+	virtual bool needsRetrieveTriangles() const
+	{
+		btAssert(0);
+		return false;
+	}
+
+	//! Determines if this shape has tetrahedrons
+	virtual bool needsRetrieveTetrahedrons() const
+	{
+		btAssert(0);
+		return false;
+	}
+
+	virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const
+	{
+		btAssert(0);
+	}
+
+	virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const
+	{
+		btAssert(0);
+	}
+
+	//! call when reading child shapes
+	virtual void lockChildShapes()
+	{
+		btAssert(0);
+	}
+
+	virtual void unlockChildShapes()
+	{
+		btAssert(0);
+	}
+
+
+
+
+	//! Retrieves the bound from a child
+    /*!
+    */
+    virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+    {
+        btAssert(0);
+    }
+
+	//! Gets the children
+	virtual btCollisionShape* getChildShape(int index)
+	{
+		btAssert(0);
+		return NULL;
+	}
+
+
+	//! Gets the child
+	virtual const btCollisionShape* getChildShape(int index) const
+	{
+		btAssert(0);
+		return NULL;
+	}
+
+	//! Gets the children transform
+	virtual btTransform	getChildTransform(int index) const
+	{
+		btAssert(0);
+		return btTransform();
+	}
+
+	//! Sets the children transform
+	/*!
+	\post You must call updateBound() for update the box set.
+	*/
+	virtual void setChildTransform(int index, const btTransform & transform)
+	{
+		btAssert(0);
+	}
+
+
+	virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType()
+	{
+		return CONST_GIMPACT_TRIMESH_SHAPE;
+	}
+
+
+	virtual const char*	getName()const
+	{
+		return "GImpactMesh";
+	}
+
+	virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback)  const;
+
+	//! Function for retrieve triangles.
+	/*!
+	It gives the triangles in local space
+	*/
+	virtual void	processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+};
+
+
+#endif //GIMPACT_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp	(revision 1963)
@@ -0,0 +1,283 @@
+/*! \file btGenericPoolAllocator.cpp
+\author Francisco Len Nßjera. email projectileman@yahoo.com
+
+General purpose allocator class
+*/
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGenericPoolAllocator.h"
+
+
+
+/// *************** btGenericMemoryPool ******************///////////
+
+size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
+{
+	size_t ptr = BT_UINT_MAX;
+
+	if(m_free_nodes_count == 0) return BT_UINT_MAX;
+	// find an avaliable free node with the correct size
+	size_t revindex = m_free_nodes_count;
+
+	while(revindex-- && ptr == BT_UINT_MAX)
+	{
+		if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements)
+		{
+			ptr = revindex;
+		}
+	}
+	if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
+
+
+	revindex = ptr;
+	ptr = m_free_nodes[revindex];
+	// post: ptr contains the node index, and revindex the index in m_free_nodes
+
+	size_t  finalsize = m_allocated_sizes[ptr];
+	finalsize -= num_elements;
+
+	m_allocated_sizes[ptr] = num_elements;
+
+	// post: finalsize>=0, m_allocated_sizes[ptr] has the requested size
+
+	if(finalsize>0) // preserve free node, there are some free memory
+	{
+		m_free_nodes[revindex] = ptr + num_elements;
+		m_allocated_sizes[ptr + num_elements] = finalsize;
+	}
+	else // delete free node
+	{
+		// swap with end
+		m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1];
+		m_free_nodes_count--;
+	}
+
+	return ptr;
+}
+
+size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements)
+{
+	if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX;
+
+	size_t ptr = m_allocated_count;
+
+	m_allocated_sizes[m_allocated_count] = num_elements;
+	m_allocated_count+=num_elements;
+
+	return ptr;
+}
+
+
+void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
+{
+	m_allocated_count = 0;
+	m_free_nodes_count = 0;
+
+	m_element_size = element_size;
+	m_max_element_count = element_count;
+
+
+
+
+	m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16);
+	m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+	m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
+
+	for (size_t i = 0;i< m_max_element_count;i++ )
+	{
+		m_allocated_sizes[i] = 0;
+	}
+}
+
+void btGenericMemoryPool::end_pool()
+{
+	btAlignedFree(m_pool);
+	btAlignedFree(m_free_nodes);
+	btAlignedFree(m_allocated_sizes);
+	m_allocated_count = 0;
+	m_free_nodes_count = 0;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericMemoryPool::allocate(size_t size_bytes)
+{
+
+	size_t module = size_bytes%m_element_size;
+	size_t element_count = size_bytes/m_element_size;
+	if(module>0) element_count++;
+
+	size_t alloc_pos = allocate_from_free_nodes(element_count);
+	// a free node is found
+	if(alloc_pos != BT_UINT_MAX)
+	{
+		return get_element_data(alloc_pos);
+	}
+	// allocate directly on pool
+	alloc_pos = allocate_from_pool(element_count);
+
+	if(alloc_pos == BT_UINT_MAX) return NULL; // not space
+	return get_element_data(alloc_pos);
+}
+
+bool btGenericMemoryPool::freeMemory(void * pointer)
+{
+	unsigned char * pointer_pos = (unsigned char *)pointer;
+	unsigned char * pool_pos = (unsigned char *)m_pool;
+	// calc offset
+	if(pointer_pos<pool_pos) return false;//other pool
+	size_t offset = size_t(pointer_pos - pool_pos);
+	if(offset>=get_pool_capacity()) return false;// far away
+
+	// find free position
+	m_free_nodes[m_free_nodes_count] = offset/m_element_size;
+	m_free_nodes_count++;
+	return true;
+}
+
+
+/// *******************! btGenericPoolAllocator *******************!///
+
+
+btGenericPoolAllocator::~btGenericPoolAllocator()
+{
+	// destroy pools
+	size_t i;
+	for (i=0;i<m_pool_count;i++)
+	{
+		m_pools[i]->end_pool();
+		btAlignedFree(m_pools[i]);
+	}
+}
+
+
+// creates a pool
+btGenericMemoryPool * btGenericPoolAllocator::push_new_pool()
+{
+	if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
+
+	btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16);
+
+	m_pools[m_pool_count] = newptr;
+
+	m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count);
+
+	m_pool_count++;
+	return newptr;
+}
+
+void * btGenericPoolAllocator::failback_alloc(size_t size_bytes)
+{
+
+	btGenericMemoryPool * pool = NULL;
+
+
+	if(size_bytes<=get_pool_capacity())
+	{
+		pool = 	push_new_pool();
+	}
+
+	if(pool==NULL) // failback
+	{
+		return btAlignedAlloc(size_bytes,16);
+	}
+
+	return pool->allocate(size_bytes);
+}
+
+bool btGenericPoolAllocator::failback_free(void * pointer)
+{
+	btAlignedFree(pointer);
+	return true;
+}
+
+
+//! Allocates memory in pool
+/*!
+\param size_bytes size in bytes of the buffer
+*/
+void * btGenericPoolAllocator::allocate(size_t size_bytes)
+{
+	void * ptr = NULL;
+
+	size_t i = 0;
+	while(i<m_pool_count && ptr == NULL)
+	{
+		ptr = m_pools[i]->allocate(size_bytes);
+		++i;
+	}
+
+	if(ptr) return ptr;
+
+	return failback_alloc(size_bytes);
+}
+
+bool btGenericPoolAllocator::freeMemory(void * pointer)
+{
+	bool result = false;
+
+	size_t i = 0;
+	while(i<m_pool_count && result == false)
+	{
+		result = m_pools[i]->freeMemory(pointer);
+		++i;
+	}
+
+	if(result) return true;
+
+	return failback_free(pointer);
+}
+
+/// ************** STANDARD ALLOCATOR ***************************///
+
+
+#define BT_DEFAULT_POOL_SIZE 32768
+#define BT_DEFAULT_POOL_ELEMENT_SIZE 8
+
+// main allocator
+class BT_STANDARD_ALLOCATOR: public btGenericPoolAllocator
+{
+public:
+	BT_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE)
+	{
+	}
+};
+
+// global allocator
+BT_STANDARD_ALLOCATOR g_main_allocator;
+
+
+void * btPoolAlloc(size_t size)
+{
+	return g_main_allocator.allocate(size);
+}
+
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
+{
+	void * newptr = btPoolAlloc(newsize);
+    size_t copysize = oldsize<newsize?oldsize:newsize;
+    memcpy(newptr,ptr,copysize);
+    btPoolFree(ptr);
+    return newptr;
+}
+
+void btPoolFree(void *ptr)
+{
+	g_main_allocator.freeMemory(ptr);
+}
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h	(revision 1963)
@@ -0,0 +1,168 @@
+/*! \file btGenericPoolAllocator.h
+\author Francisco Len Nßjera. email projectileman@yahoo.com
+
+General purpose allocator class
+*/
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GENERIC_POOL_ALLOCATOR_H
+#define BT_GENERIC_POOL_ALLOCATOR_H
+
+#include <limits.h>
+#include <stdio.h>
+#include <memory.h>
+#include "LinearMath/btAlignedAllocator.h"
+
+#define BT_UINT_MAX UINT_MAX
+#define BT_DEFAULT_MAX_POOLS 16
+
+
+//! Generic Pool class
+class btGenericMemoryPool
+{
+public:
+	unsigned char * m_pool; //[m_element_size*m_max_element_count];
+	size_t * m_free_nodes; //[m_max_element_count];//! free nodes
+	size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
+	size_t m_allocated_count;
+	size_t m_free_nodes_count;
+protected:
+	size_t m_element_size;
+	size_t m_max_element_count;
+
+	size_t allocate_from_free_nodes(size_t num_elements);
+	size_t allocate_from_pool(size_t num_elements);
+
+public:
+
+	void init_pool(size_t element_size, size_t element_count);
+
+	void end_pool();
+
+
+	btGenericMemoryPool(size_t element_size, size_t element_count)
+	{
+		init_pool(element_size, element_count);
+	}
+
+	~btGenericMemoryPool()
+	{
+		end_pool();
+	}
+
+
+	inline size_t get_pool_capacity()
+	{
+		return m_element_size*m_max_element_count;
+	}
+
+	inline size_t gem_element_size()
+	{
+		return m_element_size;
+	}
+
+	inline size_t get_max_element_count()
+	{
+		return m_max_element_count;
+	}
+
+	inline size_t get_allocated_count()
+	{
+		return m_allocated_count;
+	}
+
+	inline size_t get_free_positions_count()
+	{
+		return m_free_nodes_count;
+	}
+
+	inline void * get_element_data(size_t element_index)
+	{
+		return &m_pool[element_index*m_element_size];
+	}
+
+	//! Allocates memory in pool
+	/*!
+	\param size_bytes size in bytes of the buffer
+	*/
+	void * allocate(size_t size_bytes);
+
+	bool freeMemory(void * pointer);
+};
+
+
+
+
+//! Generic Allocator with pools
+/*!
+General purpose Allocator which can create Memory Pools dynamiacally as needed.
+*/
+class btGenericPoolAllocator
+{
+protected:
+	size_t m_pool_element_size;
+	size_t m_pool_element_count;
+public:
+	btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
+	size_t m_pool_count;
+
+
+	inline size_t get_pool_capacity()
+	{
+		return m_pool_element_size*m_pool_element_count;
+	}
+
+
+protected:
+	// creates a pool
+	btGenericMemoryPool * push_new_pool();
+
+	void * failback_alloc(size_t size_bytes);
+
+	bool failback_free(void * pointer);
+public:
+
+	btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
+	{
+		m_pool_count = 0;
+		m_pool_element_size = pool_element_size;
+		m_pool_element_count = pool_element_count;
+	}
+
+	virtual ~btGenericPoolAllocator();
+
+	//! Allocates memory in pool
+	/*!
+	\param size_bytes size in bytes of the buffer
+	*/
+	void * allocate(size_t size_bytes);
+
+	bool freeMemory(void * pointer);
+};
+
+
+
+/*! \defgroup POOL_MEMORY_FUNCTIONS
+standar managed Memory functions. Memory pools are used.
+*/
+//! @{
+void * btPoolAlloc(size_t size);
+void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
+void btPoolFree(void *ptr);
+//! @}
+
+
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btGeometryOperations.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btGeometryOperations.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btGeometryOperations.h	(revision 1963)
@@ -0,0 +1,216 @@
+#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file btGeometryOperations.h
+*\author Francisco Len Nßjera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBoxCollision.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+/// Calc a plane from a triangle edge an a normal. plane is a vec4f
+SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
+{
+	btVector3 planenormal = (e2-e1).cross(normal);
+	planenormal.normalize();
+	plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
+}
+
+
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+SIMD_FORCE_INLINE void bt_closest_point_on_segment(
+	btVector3 & cp, const btVector3 & v,
+	const btVector3  &e1,const btVector3 &e2)
+{
+    btVector3 n = e2-e1;
+    cp = v - e1;
+	btScalar _scalar = cp.dot(n)/n.dot(n);
+	if(_scalar <0.0f)
+	{
+	    cp = e1;
+	}
+	else if(_scalar >1.0f)
+	{
+	    cp = e2;
+	}
+	else
+	{
+		cp = _scalar*n + e1;
+	}
+}
+
+
+//! line plane collision
+/*!
+*\return
+	-0  if the ray never intersects
+	-1 if the ray collides in front
+	-2 if the ray collides in back
+*/
+
+SIMD_FORCE_INLINE int bt_line_plane_collision(
+	const btVector4 & plane,
+	const btVector3 & vDir,
+	const btVector3 & vPoint,
+	btVector3 & pout,
+	btScalar &tparam,
+	btScalar tmin, btScalar tmax)
+{
+
+	btScalar _dotdir = vDir.dot(plane);
+
+	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	{
+		tparam = tmax;
+	    return 0;
+	}
+
+	btScalar _dis = bt_distance_point_plane(plane,vPoint);
+	char returnvalue = _dis<0.0f? 2:1;
+	tparam = -_dis/_dotdir;
+
+	if(tparam<tmin)
+	{
+		returnvalue = 0;
+		tparam = tmin;
+	}
+	else if(tparam>tmax)
+	{
+		returnvalue = 0;
+		tparam = tmax;
+	}
+	pout = tparam*vDir + vPoint;
+	return returnvalue;
+}
+
+
+//! Find closest points on segments
+SIMD_FORCE_INLINE void bt_segment_collision(
+	const btVector3 & vA1,
+	const btVector3 & vA2,
+	const btVector3 & vB1,
+	const btVector3 & vB2,
+	btVector3 & vPointA,
+	btVector3 & vPointB)
+{
+    btVector3 AD = vA2 - vA1;
+    btVector3 BD = vB2 - vB1;
+    btVector3 N = AD.cross(BD);
+    btScalar tp = N.length2();
+
+    btVector4 _M;//plane
+
+    if(tp<SIMD_EPSILON)//ARE PARALELE
+    {
+    	//project B over A
+    	bool invert_b_order = false;
+    	_M[0] = vB1.dot(AD);
+    	_M[1] = vB2.dot(AD);
+
+    	if(_M[0]>_M[1])
+    	{
+    		invert_b_order  = true;
+    		BT_SWAP_NUMBERS(_M[0],_M[1]);
+    	}
+    	_M[2] = vA1.dot(AD);
+    	_M[3] = vA2.dot(AD);
+    	//mid points
+    	N[0] = (_M[0]+_M[1])*0.5f;
+    	N[1] = (_M[2]+_M[3])*0.5f;
+
+    	if(N[0]<N[1])
+    	{
+    		if(_M[1]<_M[2])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			vPointA = vA1;
+    		}
+    		else if(_M[1]<_M[3])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+    		}
+    		else
+    		{
+    			vPointA = vA2;
+    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+    		}
+    	}
+    	else
+    	{
+    		if(_M[3]<_M[0])
+    		{
+    			vPointB = invert_b_order?vB2:vB1;
+    			vPointA = vA2;
+    		}
+    		else if(_M[3]<_M[1])
+    		{
+    			vPointA = vA2;
+    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
+    		}
+    		else
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
+    		}
+    	}
+    	return;
+    }
+
+    N = N.cross(BD);
+    _M.setValue(N[0],N[1],N[2],vB1.dot(N));
+
+	// get point A as the plane collision point
+    bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
+
+    /*Closest point on segment*/
+    vPointB = vPointA - vB1;
+	tp = vPointB.dot(BD);
+	tp/= BD.dot(BD);
+	tp = BT_CLAMP(tp,0.0f,1.0f);
+
+	vPointB = tp*BD + vB1;
+}
+
+
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btQuantization.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btQuantization.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btQuantization.h	(revision 1963)
@@ -0,0 +1,92 @@
+#ifndef BT_QUANTIZATION_H_INCLUDED
+#define BT_QUANTIZATION_H_INCLUDED
+
+/*! \file btQuantization.h
+*\author Francisco Len Nßjera
+
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btTransform.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+
+SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
+	btVector3 & outMinBound,
+	btVector3 & outMaxBound,
+	btVector3 & bvhQuantization,
+	const btVector3& srcMinBound,const btVector3& srcMaxBound,
+	btScalar quantizationMargin)
+{
+	//enlarge the AABB to avoid division by zero when initializing the quantization values
+	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
+	outMinBound = srcMinBound - clampValue;
+	outMaxBound = srcMaxBound + clampValue;
+	btVector3 aabbSize = outMaxBound - outMinBound;
+	bvhQuantization = btVector3(btScalar(65535.0),
+								btScalar(65535.0),
+								btScalar(65535.0)) / aabbSize;
+}
+
+
+SIMD_FORCE_INLINE void bt_quantize_clamp(
+	unsigned short* out,
+	const btVector3& point,
+	const btVector3 & min_bound,
+	const btVector3 & max_bound,
+	const btVector3 & bvhQuantization)
+{
+
+	btVector3 clampedPoint(point);
+	clampedPoint.setMax(min_bound);
+	clampedPoint.setMin(max_bound);
+
+	btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
+	out[0] = (unsigned short)(v.getX()+0.5f);
+	out[1] = (unsigned short)(v.getY()+0.5f);
+	out[2] = (unsigned short)(v.getZ()+0.5f);
+}
+
+
+SIMD_FORCE_INLINE btVector3 bt_unquantize(
+	const unsigned short* vecIn,
+	const btVector3 & offset,
+	const btVector3 & bvhQuantization)
+{
+	btVector3	vecOut;
+	vecOut.setValue(
+		(btScalar)(vecIn[0]) / (bvhQuantization.getX()),
+		(btScalar)(vecIn[1]) / (bvhQuantization.getY()),
+		(btScalar)(vecIn[2]) / (bvhQuantization.getZ()));
+	vecOut += offset;
+	return vecOut;
+}
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp	(revision 1963)
@@ -0,0 +1,218 @@
+/*! \file btGImpactTriangleShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btTriangleShapeEx.h"
+
+
+
+void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
+                                       btScalar margin, const btVector3 * points, int point_count)
+{
+    m_point_count = 0;
+    m_penetration_depth= -1000.0f;
+
+    int point_indices[MAX_TRI_CLIPPING];
+
+	int _k;
+
+    for ( _k=0;_k<point_count;_k++)
+    {
+        btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
+
+        if (_dist>=0.0f)
+        {
+            if (_dist>m_penetration_depth)
+            {
+                m_penetration_depth = _dist;
+                point_indices[0] = _k;
+                m_point_count=1;
+            }
+            else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
+            {
+                point_indices[m_point_count] = _k;
+                m_point_count++;
+            }
+        }
+    }
+
+    for ( _k=0;_k<m_point_count;_k++)
+    {
+        m_points[_k] = points[point_indices[_k]];
+    }
+}
+
+///class btPrimitiveTriangle
+bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
+{
+    btScalar total_margin = m_margin + other.m_margin;
+    // classify points on other triangle
+    btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
+
+    btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
+
+    btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    // classify points on this triangle
+    dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
+
+    dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
+
+    dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    return true;
+}
+
+int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
+{
+    // edge 0
+
+    btVector3 temp_points[MAX_TRI_CLIPPING];
+
+
+    btVector4 edgeplane;
+
+    get_edge_plane(0,edgeplane);
+
+
+    int clipped_count = bt_plane_clip_triangle(
+                            edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
+
+    if (clipped_count == 0) return 0;
+
+    btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+
+    // edge 1
+    get_edge_plane(1,edgeplane);
+
+
+    clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1);
+
+    if (clipped_count == 0) return 0;
+
+    // edge 2
+    get_edge_plane(2,edgeplane);
+
+    clipped_count = bt_plane_clip_polygon(
+                        edgeplane,temp_points1,clipped_count,clipped_points);
+
+    return clipped_count;
+}
+
+bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts)
+{
+    btScalar margin = m_margin + other.m_margin;
+
+    btVector3 clipped_points[MAX_TRI_CLIPPING];
+    int clipped_count;
+    //create planes
+    // plane v vs U points
+
+    BT_TRIANGLE_CONTACT contacts1;
+
+    contacts1.m_separating_normal = m_plane;
+
+
+    clipped_count = clip_triangle(other,clipped_points);
+
+    if (clipped_count == 0 )
+    {
+        return false;//Reject
+    }
+
+    //find most deep interval face1
+    contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+    if (contacts1.m_point_count == 0) return false; // too far
+    //Normal pointing to this triangle
+    contacts1.m_separating_normal *= -1.f;
+
+
+    //Clip tri1 by tri2 edges
+    BT_TRIANGLE_CONTACT contacts2;
+    contacts2.m_separating_normal = other.m_plane;
+
+    clipped_count = other.clip_triangle(*this,clipped_points);
+
+    if (clipped_count == 0 )
+    {
+        return false;//Reject
+    }
+
+    //find most deep interval face1
+    contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+    if (contacts2.m_point_count == 0) return false; // too far
+
+
+
+
+    ////check most dir for contacts
+    if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+    {
+        contacts.copy_from(contacts2);
+    }
+    else
+    {
+        contacts.copy_from(contacts1);
+    }
+    return true;
+}
+
+
+
+///class btTriangleShapeEx: public btTriangleShape
+
+bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
+{
+    btScalar total_margin = getMargin() + other.getMargin();
+
+    btVector4 plane0;
+    buildTriPlane(plane0);
+    btVector4 plane1;
+    other.buildTriPlane(plane1);
+
+    // classify points on other triangle
+    btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
+
+    btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
+
+    btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    // classify points on this triangle
+    dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
+
+    dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
+
+    dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
+
+    if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
+
+    return true;
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h	(revision 1963)
@@ -0,0 +1,180 @@
+/*! \file btGImpactShape.h
+\author Francisco Len Nßjera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef TRIANGLE_SHAPE_EX_H
+#define TRIANGLE_SHAPE_EX_H
+
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "btBoxCollision.h"
+#include "btClipPolygon.h"
+#include "btGeometryOperations.h"
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct BT_TRIANGLE_CONTACT
+{
+    btScalar m_penetration_depth;
+    int m_point_count;
+    btVector4 m_separating_normal;
+    btVector3 m_points[MAX_TRI_CLIPPING];
+
+	SIMD_FORCE_INLINE void copy_from(const BT_TRIANGLE_CONTACT& other)
+	{
+		m_penetration_depth = other.m_penetration_depth;
+		m_separating_normal = other.m_separating_normal;
+		m_point_count = other.m_point_count;
+		int i = m_point_count;
+		while(i--)
+		{
+			m_points[i] = other.m_points[i];
+		}
+	}
+
+	BT_TRIANGLE_CONTACT()
+	{
+	}
+
+	BT_TRIANGLE_CONTACT(const BT_TRIANGLE_CONTACT& other)
+	{
+		copy_from(other);
+	}
+
+    //! classify points that are closer
+    void merge_points(const btVector4 & plane,
+    				btScalar margin, const btVector3 * points, int point_count);
+
+};
+
+
+
+class btPrimitiveTriangle
+{
+public:
+	btVector3 m_vertices[3];
+	btVector4 m_plane;
+	btScalar m_margin;
+	btScalar m_dummy;
+	btPrimitiveTriangle():m_margin(0.01f)
+	{
+
+	}
+
+
+	SIMD_FORCE_INLINE void buildTriPlane()
+	{
+		btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
+		normal.normalize();
+		m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
+	}
+
+	//! Test if triangles could collide
+	bool overlap_test_conservative(const btPrimitiveTriangle& other);
+
+	//! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane
+	/*!
+	\pre this triangle must have its plane calculated.
+	*/
+	SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane)  const
+    {
+		const btVector3 & e0 = m_vertices[edge_index];
+		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+		bt_edge_plane(e0,e1,m_plane,plane);
+    }
+
+    void applyTransform(const btTransform& t)
+	{
+		m_vertices[0] = t(m_vertices[0]);
+		m_vertices[1] = t(m_vertices[1]);
+		m_vertices[2] = t(m_vertices[2]);
+	}
+
+	//! Clips the triangle against this
+	/*!
+	\pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
+	\return the number of clipped points
+	*/
+    int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );
+
+	//! Find collision using the clipping method
+	/*!
+	\pre this triangle and other must have their triangles calculated
+	*/
+    bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_TRIANGLE_CONTACT & contacts);
+};
+
+
+
+//! Helper class for colliding Bullet Triangle Shapes
+/*!
+This class implements a better getAabb method than the previous btTriangleShape class
+*/
+class btTriangleShapeEx: public btTriangleShape
+{
+public:
+
+	btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
+	{
+	}
+
+	btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2):	btTriangleShape(p0,p1,p2)
+	{
+	}
+
+	btTriangleShapeEx(const btTriangleShapeEx & other):	btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
+	{
+	}
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
+	{
+		btVector3 tv0 = t(m_vertices1[0]);
+		btVector3 tv1 = t(m_vertices1[1]);
+		btVector3 tv2 = t(m_vertices1[2]);
+
+		btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
+		aabbMin = trianglebox.m_min;
+		aabbMax = trianglebox.m_max;
+	}
+
+	void applyTransform(const btTransform& t)
+	{
+		m_vertices1[0] = t(m_vertices1[0]);
+		m_vertices1[1] = t(m_vertices1[1]);
+		m_vertices1[2] = t(m_vertices1[2]);
+	}
+
+	SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
+	{
+		btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
+		normal.normalize();
+		plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
+	}
+
+	bool overlap_test_conservative(const btTriangleShapeEx& other);
+};
+
+
+#endif //TRIANGLE_MESH_SHAPE_H
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_array.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_array.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_array.h	(revision 1963)
@@ -0,0 +1,332 @@
+#ifndef GIM_ARRAY_H_INCLUDED
+#define GIM_ARRAY_H_INCLUDED
+/*! \file gim_array.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for template containers
+*/
+//! @{
+
+#define GIM_ARRAY_GROW_INCREMENT 2
+#define GIM_ARRAY_GROW_FACTOR 2
+
+//!	Very simple array container with fast access and simd memory
+template<typename T>
+class gim_array
+{
+public:
+//! properties
+//!@{
+    T *m_data;
+    GUINT m_size;
+    GUINT m_allocated_size;
+//!@}
+//! protected operations
+//!@{
+
+    inline void destroyData()
+	{
+	    m_allocated_size = 0;
+		if(m_data==NULL) return;
+		gim_free(m_data);
+		m_data = NULL;
+	}
+
+	inline bool resizeData(GUINT newsize)
+	{
+		if(newsize==0)
+		{
+			destroyData();
+			return true;
+		}
+
+		if(m_size>0)
+		{
+            m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
+		}
+		else
+		{
+		    m_data = (T*)gim_alloc(newsize*sizeof(T));
+		}
+		m_allocated_size = newsize;
+		return true;
+	}
+
+	inline bool growingCheck()
+	{
+		if(m_allocated_size<=m_size)
+		{
+		    GUINT requestsize = m_size;
+		    m_size = m_allocated_size;
+			if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
+		}
+		return true;
+	}
+
+//!@}
+//! public operations
+//!@{
+    inline  bool reserve(GUINT size)
+    {
+        if(m_allocated_size>=size) return false;
+        return resizeData(size);
+    }
+
+    inline void clear_range(GUINT start_range)
+    {
+        while(m_size>start_range)
+        {
+            m_data[--m_size].~T();
+        }
+    }
+
+    inline void clear()
+    {
+        if(m_size==0)return;
+        clear_range(0);
+    }
+
+    inline void clear_memory()
+    {
+        clear();
+        destroyData();
+    }
+
+    gim_array()
+    {
+        m_data = 0;
+        m_size = 0;
+        m_allocated_size = 0;
+    }
+
+    gim_array(GUINT reservesize)
+    {
+        m_data = 0;
+        m_size = 0;
+
+        m_allocated_size = 0;
+        reserve(reservesize);
+    }
+
+    ~gim_array()
+    {
+        clear_memory();
+    }
+
+    inline GUINT size() const
+    {
+        return m_size;
+    }
+
+    inline GUINT max_size() const
+    {
+        return m_allocated_size;
+    }
+
+    inline T & operator[](size_t i)
+	{
+		return m_data[i];
+	}
+	inline  const T & operator[](size_t i) const
+	{
+		return m_data[i];
+	}
+
+    inline T * pointer(){ return m_data;}
+    inline const T * pointer() const
+    { return m_data;}
+
+
+    inline T * get_pointer_at(GUINT i)
+	{
+		return m_data + i;
+	}
+
+	inline const T * get_pointer_at(GUINT i) const
+	{
+		return m_data + i;
+	}
+
+	inline T & at(GUINT i)
+	{
+		return m_data[i];
+	}
+
+	inline const T & at(GUINT i) const
+	{
+		return m_data[i];
+	}
+
+	inline T & front()
+	{
+		return *m_data;
+	}
+
+	inline const T & front() const
+	{
+		return *m_data;
+	}
+
+	inline T & back()
+	{
+		return m_data[m_size-1];
+	}
+
+	inline const T & back() const
+	{
+		return m_data[m_size-1];
+	}
+
+
+	inline void swap(GUINT i, GUINT j)
+	{
+	    gim_swap_elements(m_data,i,j);
+	}
+
+	inline void push_back(const T & obj)
+	{
+	    this->growingCheck();
+	    m_data[m_size] = obj;
+	    m_size++;
+	}
+
+	//!Simply increase the m_size, doesn't call the new element constructor
+	inline void push_back_mem()
+	{
+	    this->growingCheck();
+	    m_size++;
+	}
+
+	inline void push_back_memcpy(const T & obj)
+	{
+	    this->growingCheck();
+	    irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
+	    m_size++;
+	}
+
+	inline void pop_back()
+	{
+	    m_size--;
+        m_data[m_size].~T();
+	}
+
+	//!Simply decrease the m_size, doesn't call the deleted element destructor
+	inline void pop_back_mem()
+	{
+	    m_size--;
+	}
+
+    //! fast erase
+	inline void erase(GUINT index)
+	{
+	    if(index<m_size-1)
+	    {
+	        swap(index,m_size-1);
+	    }
+	    pop_back();
+	}
+
+	inline void erase_sorted_mem(GUINT index)
+	{
+	    m_size--;
+	    for(GUINT i = index;i<m_size;i++)
+	    {
+	        gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
+	    }
+	}
+
+	inline void erase_sorted(GUINT index)
+	{
+	    m_data[index].~T();
+	    erase_sorted_mem(index);
+	}
+
+	inline void insert_mem(GUINT index)
+	{
+	    this->growingCheck();
+	    for(GUINT i = m_size;i>index;i--)
+	    {
+	        gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
+	    }
+	    m_size++;
+	}
+
+	inline void insert(const T & obj,GUINT index)
+	{
+	    insert_mem(index);
+	    m_data[index] = obj;
+	}
+
+	inline void resize(GUINT size, bool call_constructor = true)
+	{
+
+	    if(size>m_size)
+	    {
+            reserve(size);
+            if(call_constructor)
+            {
+            	T obj;
+                while(m_size<size)
+                {
+                    m_data[m_size] = obj;
+                    m_size++;
+                }
+            }
+            else
+            {
+            	m_size = size;
+            }
+	    }
+	    else if(size<m_size)
+	    {
+	        if(call_constructor) clear_range(size);
+	        m_size = size;
+	    }
+	}
+
+	inline void refit()
+	{
+	    resizeData(m_size);
+	}
+//!@}
+};
+
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h	(revision 1963)
@@ -0,0 +1,547 @@
+#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
+
+/*! \file gim_basic_geometry_operations.h
+*\author Francisco Len Nßjera
+type independant geometry routines
+
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_linear_math.h"
+
+
+
+/*! \defgroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+#define PLANEDIREPSILON 0.0000001f
+#define PARALELENORMALS 0.000001f
+
+
+#define TRIANGLE_NORMAL(v1,v2,v3,n)\
+{\
+	vec3f _dif1,_dif2;\
+    VEC_DIFF(_dif1,v2,v1);\
+    VEC_DIFF(_dif2,v3,v1);\
+    VEC_CROSS(n,_dif1,_dif2);\
+    VEC_NORMALIZE(n);\
+}\
+
+#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
+    vec3f _dif1,_dif2; \
+    VEC_DIFF(_dif1,v2,v1); \
+    VEC_DIFF(_dif2,v3,v1); \
+    VEC_CROSS(n,_dif1,_dif2); \
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
+    TRIANGLE_NORMAL(v1,v2,v3,plane);\
+    plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// plane is a vec4f
+#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
+    TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
+    plane[3] = VEC_DOT(v1,plane);\
+}\
+
+/// Calc a plane from an edge an a normal. plane is a vec4f
+#define EDGE_PLANE(e1,e2,n,plane) {\
+    vec3f _dif; \
+    VEC_DIFF(_dif,e2,e1); \
+    VEC_CROSS(plane,_dif,n); \
+    VEC_NORMALIZE(plane); \
+    plane[3] = VEC_DOT(e1,plane);\
+}\
+
+#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
+
+#define PROJECT_POINT_PLANE(point,plane,projected) {\
+	GREAL _dis;\
+	_dis = DISTANCE_PLANE_POINT(plane,point);\
+	VEC_SCALE(projected,-_dis,plane);\
+	VEC_SUM(projected,projected,point);	\
+}\
+
+//! Verifies if a point is in the plane hull
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool POINT_IN_HULL(
+	const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
+{
+	GREAL _dis;
+	for (GUINT _i = 0;_i< plane_count;++_i)
+	{
+		_dis = DISTANCE_PLANE_POINT(planes[_i],point);
+	    if(_dis>0.0f) return false;
+	}
+	return true;
+}
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+	GREAL _dis1,_dis2;
+	_dis1 = DISTANCE_PLANE_POINT(plane,s1);
+	VEC_DIFF(clipped,s2,s1);
+	_dis2 = VEC_DOT(clipped,plane);
+	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+	VEC_SUM(clipped,clipped,s1);
+}
+
+enum ePLANE_INTERSECTION_TYPE
+{
+	G_BACK_PLANE = 0,
+	G_COLLIDE_PLANE,
+	G_FRONT_PLANE
+};
+
+enum eLINE_PLANE_INTERSECTION_TYPE
+{
+	G_FRONT_PLANE_S1 = 0,
+	G_FRONT_PLANE_S2,
+	G_BACK_PLANE_S1,
+	G_BACK_PLANE_S2,
+	G_COLLIDE_PLANE_S1,
+	G_COLLIDE_PLANE_S2
+};
+
+//! Confirms if the plane intersect the edge or nor
+/*!
+intersection type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,
+	const CLASS_PLANE &plane,CLASS_POINT &clipped)
+{
+	GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
+	GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
+	if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
+	{
+	    if(_dis1<_dis2) return G_FRONT_PLANE_S1;
+	    return G_FRONT_PLANE_S2;
+	}
+	else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
+	{
+	    if(_dis1>_dis2) return G_BACK_PLANE_S1;
+	    return G_BACK_PLANE_S2;
+	}
+
+	VEC_DIFF(clipped,s2,s1);
+	_dis2 = VEC_DOT(clipped,plane);
+	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
+	VEC_SUM(clipped,clipped,s1);
+	if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
+	return G_COLLIDE_PLANE_S2;
+}
+
+//! Confirms if the plane intersect the edge or not
+/*!
+clipped1 and clipped2 are the vertices behind the plane.
+clipped1 is the closest
+
+intersection_type must have the following values
+<ul>
+<li> 0 : Segment in front of plane, s1 closest
+<li> 1 : Segment in front of plane, s2 closest
+<li> 2 : Segment in back of plane, s1 closest
+<li> 3 : Segment in back of plane, s2 closest
+<li> 4 : Segment collides plane, s1 in back
+<li> 5 : Segment collides plane, s2 in back
+</ul>
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
+	const CLASS_POINT& s1,
+	const CLASS_POINT &s2,
+	const CLASS_PLANE &plane,
+	CLASS_POINT &clipped1,CLASS_POINT &clipped2)
+{
+	eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
+	switch(intersection_type)
+	{
+	case G_FRONT_PLANE_S1:
+		VEC_COPY(clipped1,s1);
+	    VEC_COPY(clipped2,s2);
+		break;
+	case G_FRONT_PLANE_S2:
+		VEC_COPY(clipped1,s2);
+	    VEC_COPY(clipped2,s1);
+		break;
+	case G_BACK_PLANE_S1:
+		VEC_COPY(clipped1,s1);
+	    VEC_COPY(clipped2,s2);
+		break;
+	case G_BACK_PLANE_S2:
+		VEC_COPY(clipped1,s2);
+	    VEC_COPY(clipped2,s1);
+		break;
+	case G_COLLIDE_PLANE_S1:
+		VEC_COPY(clipped2,s1);
+		break;
+	case G_COLLIDE_PLANE_S2:
+		VEC_COPY(clipped2,s2);
+		break;
+	}
+	return intersection_type;
+}
+
+
+//! Finds the 2 smallest cartesian coordinates of a plane normal
+#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
+
+//! Ray plane collision in one way
+/*!
+Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
+It uses the PLANEDIREPSILON constant.
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
+	const CLASS_PLANE & plane,
+	const CLASS_POINT & vDir,
+	const CLASS_POINT & vPoint,
+	CLASS_POINT & pout,T &tparam)
+{
+	GREAL _dis,_dotdir;
+	_dotdir = VEC_DOT(plane,vDir);
+	if(_dotdir<PLANEDIREPSILON)
+	{
+	    return false;
+	}
+	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
+	tparam = -_dis/_dotdir;
+	VEC_SCALE(pout,tparam,vDir);
+	VEC_SUM(pout,vPoint,pout);
+	return true;
+}
+
+//! line collision
+/*!
+*\return
+	-0  if the ray never intersects
+	-1 if the ray collides in front
+	-2 if the ray collides in back
+*/
+template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
+	const CLASS_PLANE & plane,
+	const CLASS_POINT & vDir,
+	const CLASS_POINT & vPoint,
+	CLASS_POINT & pout,
+	T &tparam,
+	T tmin, T tmax)
+{
+	GREAL _dis,_dotdir;
+	_dotdir = VEC_DOT(plane,vDir);
+	if(btFabs(_dotdir)<PLANEDIREPSILON)
+	{
+		tparam = tmax;
+	    return 0;
+	}
+	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
+	char returnvalue = _dis<0.0f?2:1;
+	tparam = -_dis/_dotdir;
+
+	if(tparam<tmin)
+	{
+		returnvalue = 0;
+		tparam = tmin;
+	}
+	else if(tparam>tmax)
+	{
+		returnvalue = 0;
+		tparam = tmax;
+	}
+
+	VEC_SCALE(pout,tparam,vDir);
+	VEC_SUM(pout,vPoint,pout);
+	return returnvalue;
+}
+
+/*! \brief Returns the Ray on which 2 planes intersect if they do.
+    Written by Rodrigo Hernandez on ODE convex collision
+
+  \param p1 Plane 1
+  \param p2 Plane 2
+  \param p Contains the origin of the ray upon returning if planes intersect
+  \param d Contains the direction of the ray upon returning if planes intersect
+  \return true if the planes intersect, 0 if paralell.
+
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE bool INTERSECT_PLANES(
+		const CLASS_PLANE &p1,
+		const CLASS_PLANE &p2,
+		CLASS_POINT &p,
+		CLASS_POINT &d)
+{
+	VEC_CROSS(d,p1,p2);
+  	GREAL denom = VEC_DOT(d, d);
+  	if(GIM_IS_ZERO(denom)) return false;
+	vec3f _n;
+	_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
+	_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
+	_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
+	VEC_CROSS(p,_n,d);
+	p[0]/=denom;
+	p[1]/=denom;
+	p[2]/=denom;
+	return true;
+}
+
+//***************** SEGMENT and LINE FUNCTIONS **********************************///
+
+/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
+ */
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
+	CLASS_POINT & cp, const CLASS_POINT & v,
+	const CLASS_POINT &e1,const CLASS_POINT &e2)
+{
+    vec3f _n;
+    VEC_DIFF(_n,e2,e1);
+    VEC_DIFF(cp,v,e1);
+	GREAL _scalar = VEC_DOT(cp, _n);
+	_scalar/= VEC_DOT(_n, _n);
+	if(_scalar <0.0f)
+	{
+	    VEC_COPY(cp,e1);
+	}
+	else if(_scalar >1.0f)
+	{
+	    VEC_COPY(cp,e2);
+	}
+	else
+	{
+        VEC_SCALE(cp,_scalar,_n);
+        VEC_SUM(cp,cp,e1);
+	}
+}
+
+
+/*! \brief Finds the line params where these lines intersect.
+
+\param dir1 Direction of line 1
+\param point1 Point of line 1
+\param dir2 Direction of line 2
+\param point2 Point of line 2
+\param t1 Result Parameter for line 1
+\param t2 Result Parameter for line 2
+\param dointersect  0  if the lines won't intersect, else 1
+
+*/
+template<typename T,typename CLASS_POINT>
+SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
+	const CLASS_POINT & dir1,
+	CLASS_POINT & point1,
+	const CLASS_POINT & dir2,
+	CLASS_POINT &  point2,
+	T& t1,T& t2)
+{
+    GREAL det;
+	GREAL e1e1 = VEC_DOT(dir1,dir1);
+	GREAL e1e2 = VEC_DOT(dir1,dir2);
+	GREAL e2e2 = VEC_DOT(dir2,dir2);
+	vec3f p1p2;
+    VEC_DIFF(p1p2,point1,point2);
+    GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
+	GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
+	det = e1e2*e1e2 - e1e1*e2e2;
+	if(GIM_IS_ZERO(det)) return false;
+	t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
+	t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
+	return true;
+}
+
+//! Find closest points on segments
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void SEGMENT_COLLISION(
+	const CLASS_POINT & vA1,
+	const CLASS_POINT & vA2,
+	const CLASS_POINT & vB1,
+	const CLASS_POINT & vB2,
+	CLASS_POINT & vPointA,
+	CLASS_POINT & vPointB)
+{
+    CLASS_POINT _AD,_BD,_N;
+    vec4f _M;//plane
+    VEC_DIFF(_AD,vA2,vA1);
+    VEC_DIFF(_BD,vB2,vB1);
+    VEC_CROSS(_N,_AD,_BD);
+    GREAL _tp = VEC_DOT(_N,_N);
+    if(_tp<G_EPSILON)//ARE PARALELE
+    {
+    	//project B over A
+    	bool invert_b_order = false;
+    	_M[0] = VEC_DOT(vB1,_AD);
+    	_M[1] = VEC_DOT(vB2,_AD);
+    	if(_M[0]>_M[1])
+    	{
+    		invert_b_order  = true;
+    		GIM_SWAP_NUMBERS(_M[0],_M[1]);
+    	}
+    	_M[2] = VEC_DOT(vA1,_AD);
+    	_M[3] = VEC_DOT(vA2,_AD);
+    	//mid points
+    	_N[0] = (_M[0]+_M[1])*0.5f;
+    	_N[1] = (_M[2]+_M[3])*0.5f;
+
+    	if(_N[0]<_N[1])
+    	{
+    		if(_M[1]<_M[2])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			vPointA = vA1;
+    		}
+    		else if(_M[1]<_M[3])
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+    		}
+    		else
+    		{
+    			vPointA = vA2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+    		}
+    	}
+    	else
+    	{
+    		if(_M[3]<_M[0])
+    		{
+    			vPointB = invert_b_order?vB2:vB1;
+    			vPointA = vA2;
+    		}
+    		else if(_M[3]<_M[1])
+    		{
+    			vPointA = vA2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
+    		}
+    		else
+    		{
+    			vPointB = invert_b_order?vB1:vB2;
+    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
+    		}
+    	}
+    	return;
+    }
+
+
+    VEC_CROSS(_M,_N,_BD);
+    _M[3] = VEC_DOT(_M,vB1);
+
+    LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
+    /*Closest point on segment*/
+    VEC_DIFF(vPointB,vPointA,vB1);
+	_tp = VEC_DOT(vPointB, _BD);
+	_tp/= VEC_DOT(_BD, _BD);
+	_tp = GIM_CLAMP(_tp,0.0f,1.0f);
+    VEC_SCALE(vPointB,_tp,_BD);
+    VEC_SUM(vPointB,vPointB,vB1);
+}
+
+
+
+
+//! Line box intersection in one dimension
+/*!
+
+*\param pos Position of the ray
+*\param dir Projection of the Direction of the ray
+*\param bmin Minimum bound of the box
+*\param bmax Maximum bound of the box
+*\param tfirst the minimum projection. Assign to 0 at first.
+*\param tlast the maximum projection. Assign to INFINITY at first.
+*\return true if there is an intersection.
+*/
+template<typename T>
+SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
+{
+	if(GIM_IS_ZERO(dir))
+	{
+        return !(pos < bmin || pos > bmax);
+	}
+	GREAL a0 = (bmin - pos) / dir;
+	GREAL a1 = (bmax - pos) / dir;
+	if(a0 > a1)   GIM_SWAP_NUMBERS(a0, a1);
+	tfirst = GIM_MAX(a0, tfirst);
+	tlast = GIM_MIN(a1, tlast);
+	if (tlast < tfirst) return false;
+	return true;
+}
+
+
+//! Sorts 3 componets
+template<typename T>
+SIMD_FORCE_INLINE void SORT_3_INDICES(
+		const T * values,
+		GUINT * order_indices)
+{
+	//get minimum
+	order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
+
+	//get second and third
+	GUINT i0 = (order_indices[0] + 1)%3;
+	GUINT i1 = (i0 + 1)%3;
+
+	if(values[i0] < values[i1])
+	{
+		order_indices[1] = i0;
+		order_indices[2] = i1;
+	}
+	else
+	{
+		order_indices[1] = i1;
+		order_indices[2] = i0;
+	}
+}
+
+
+
+//! @}
+
+
+#endif // GIM_VECTOR_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_bitset.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_bitset.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_bitset.h	(revision 1963)
@@ -0,0 +1,129 @@
+#ifndef GIM_BITSET_H_INCLUDED
+#define GIM_BITSET_H_INCLUDED
+/*! \file gim_bitset.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_array.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for template containers
+*/
+//! @{
+
+#define GUINT_BIT_COUNT 32
+#define GUINT_EXPONENT 5
+
+class gim_bitset
+{
+public:
+    gim_array<GUINT> m_container;
+
+    gim_bitset()
+    {
+
+    }
+
+    gim_bitset(GUINT bits_count)
+    {
+        resize(bits_count);
+    }
+
+    ~gim_bitset()
+    {
+    }
+
+	inline bool resize(GUINT newsize)
+	{
+		GUINT oldsize = m_container.size();
+		m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
+		while(oldsize<m_container.size())
+		{
+			m_container[oldsize] = 0;
+		}
+		return true;
+	}
+
+	inline GUINT size()
+	{
+		return m_container.size()*GUINT_BIT_COUNT;
+	}
+
+	inline void set_all()
+	{
+		for(GUINT i = 0;i<m_container.size();++i)
+		{
+			m_container[i] = 0xffffffff;
+		}
+	}
+
+	inline void clear_all()
+	{
+	    for(GUINT i = 0;i<m_container.size();++i)
+		{
+			m_container[i] = 0;
+		}
+	}
+
+	inline void set(GUINT bit_index)
+	{
+		if(bit_index>=size())
+		{
+			resize(bit_index);
+		}
+		m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+	}
+
+	///Return 0 or 1
+	inline char get(GUINT bit_index)
+	{
+		if(bit_index>=size())
+		{
+			return 0;
+		}
+		char value = m_container[bit_index >> GUINT_EXPONENT] &
+					 (1 << (bit_index & (GUINT_BIT_COUNT-1)));
+		return value;
+	}
+
+	inline void clear(GUINT bit_index)
+	{
+	    m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
+	}
+};
+
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_collision.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_collision.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_collision.h	(revision 1963)
@@ -0,0 +1,594 @@
+#ifndef GIM_BOX_COLLISION_H_INCLUDED
+#define GIM_BOX_COLLISION_H_INCLUDED
+
+/*! \file gim_box_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_basic_geometry_operations.h"
+#include "LinearMath/btTransform.h"
+
+/*! \defgroup BOUND_AABB_OPERATIONS
+*/
+//! @{
+
+
+//SIMD_FORCE_INLINE bool test_cross_edge_box(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, const btVector3 & extend,
+//	int dir_index0,
+//	int dir_index1
+//	int component_index0,
+//	int component_index1)
+//{
+//	// dir coords are -z and y
+//
+//	const btScalar dir0 = -edge[dir_index0];
+//	const btScalar dir1 = edge[dir_index1];
+//	btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
+//	btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
+//	//find minmax
+//	if(pmin>pmax)
+//	{
+//		GIM_SWAP_NUMBERS(pmin,pmax);
+//	}
+//	//find extends
+//	const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
+//					extend[component_index1] * absolute_edge[dir_index1];
+//
+//	if(pmin>rad || -rad>pmax) return false;
+//	return true;
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
+//}
+//
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
+//}
+//
+//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
+//	const btVector3 & edge,
+//	const btVector3 & absolute_edge,
+//	const btVector3 & pointa,
+//	const btVector3 & pointb, btVector3 & extend)
+//{
+//
+//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
+//}
+
+#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
+{\
+	const btScalar dir0 = -edge[i_dir_0];\
+	const btScalar dir1 = edge[i_dir_1];\
+	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
+	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
+	if(pmin>pmax)\
+	{\
+		GIM_SWAP_NUMBERS(pmin,pmax); \
+	}\
+	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
+	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
+	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
+	if(pmin>rad || -rad>pmax) return false;\
+}\
+
+
+#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
+}\
+
+#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
+{\
+	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
+}\
+
+
+
+//!  Class for transforming a model1 to the space of model0
+class GIM_BOX_BOX_TRANSFORM_CACHE
+{
+public:
+    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
+	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
+	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
+
+	SIMD_FORCE_INLINE void calc_absolute_matrix()
+	{
+		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
+		m_AR[0] = vepsi + m_R1to0[0].absolute();
+		m_AR[1] = vepsi + m_R1to0[1].absolute();
+		m_AR[2] = vepsi + m_R1to0[2].absolute();
+	}
+
+	GIM_BOX_BOX_TRANSFORM_CACHE()
+	{
+	}
+
+
+	GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
+	{
+		COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
+        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
+		calc_absolute_matrix();
+	}
+
+	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
+	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
+	{
+
+		m_R1to0 = trans0.getBasis().transpose();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	//! Calcs the full invertion of the matrices. Useful for scaling matrices
+	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
+	{
+		m_R1to0 = trans0.getBasis().inverse();
+		m_T1to0 = m_R1to0 * (-trans0.getOrigin());
+
+		m_T1to0 += m_R1to0*trans1.getOrigin();
+		m_R1to0 *= trans1.getBasis();
+
+		calc_absolute_matrix();
+	}
+
+	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
+	{
+		return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
+			m_R1to0[1].dot(point) + m_T1to0.y(),
+			m_R1to0[2].dot(point) + m_T1to0.z());
+	}
+};
+
+
+#define BOX_PLANE_EPSILON 0.000001f
+
+//! Axis aligned box
+class GIM_AABB
+{
+public:
+	btVector3 m_min;
+	btVector3 m_max;
+
+	GIM_AABB()
+	{}
+
+
+	GIM_AABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	GIM_AABB(const btVector3 & V1,
+			 const btVector3 & V2,
+			 const btVector3 & V3,
+			 GREAL margin)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	GIM_AABB(const GIM_AABB &other):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+	}
+
+	GIM_AABB(const GIM_AABB &other,btScalar margin ):
+		m_min(other.m_min),m_max(other.m_max)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void invalidate()
+	{
+		m_min[0] = G_REAL_INFINITY;
+		m_min[1] = G_REAL_INFINITY;
+		m_min[2] = G_REAL_INFINITY;
+		m_max[0] = -G_REAL_INFINITY;
+		m_max[1] = -G_REAL_INFINITY;
+		m_max[2] = -G_REAL_INFINITY;
+	}
+
+	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
+	{
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
+	{
+		m_min[0] = other.m_min[0] - margin;
+		m_min[1] = other.m_min[1] - margin;
+		m_min[2] = other.m_min[2] - margin;
+
+		m_max[0] = other.m_max[0] + margin;
+		m_max[1] = other.m_max[1] + margin;
+		m_max[2] = other.m_max[2] + margin;
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+	}
+
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void calc_from_triangle_margin(
+							const CLASS_POINT & V1,
+							const CLASS_POINT & V2,
+							const CLASS_POINT & V3, btScalar margin)
+	{
+		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
+		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
+		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
+
+		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
+		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
+		m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
+
+		m_min[0] -= margin;
+		m_min[1] -= margin;
+		m_min[2] -= margin;
+		m_max[0] += margin;
+		m_max[1] += margin;
+		m_max[2] += margin;
+	}
+
+	//! Apply a transform to an AABB
+	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extends = m_max - center;
+		// Compute new center
+		center = trans(center);
+
+		btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
+ 				 extends.dot(trans.getBasis().getRow(1).absolute()),
+				 extends.dot(trans.getBasis().getRow(2).absolute()));
+
+		m_min = center - textends;
+		m_max = center + textends;
+	}
+
+	//! Merges a Box
+	SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
+	{
+		m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
+		m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
+		m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
+
+		m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
+		m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
+		m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
+	}
+
+	//! Merges a point
+	template<typename CLASS_POINT>
+	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
+	{
+		m_min[0] = GIM_MIN(m_min[0],point[0]);
+		m_min[1] = GIM_MIN(m_min[1],point[1]);
+		m_min[2] = GIM_MIN(m_min[2],point[2]);
+
+		m_max[0] = GIM_MAX(m_max[0],point[0]);
+		m_max[1] = GIM_MAX(m_max[1],point[1]);
+		m_max[2] = GIM_MAX(m_max[2],point[2]);
+	}
+
+	//! Gets the extend and center
+	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
+	{
+		center = (m_max+m_min)*0.5f;
+		extend = m_max - center;
+	}
+
+	//! Finds the intersecting box between this box and the other.
+	SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection)  const
+	{
+		intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
+		intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
+		intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
+
+		intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
+		intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
+		intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
+	}
+
+
+	SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
+	{
+		if(m_min[0] > other.m_max[0] ||
+		   m_max[0] < other.m_min[0] ||
+		   m_min[1] > other.m_max[1] ||
+		   m_max[1] < other.m_min[1] ||
+		   m_min[2] > other.m_max[2] ||
+		   m_max[2] < other.m_min[2])
+		{
+			return false;
+		}
+		return true;
+	}
+
+	/*! \brief Finds the Ray intersection parameter.
+	\param aabb Aligned box
+	\param vorigin A vec3f with the origin of the ray
+	\param vdir A vec3f with the direction of the ray
+	*/
+	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
+	{
+		btVector3 extents,center;
+		this->get_center_extend(center,extents);;
+
+		btScalar Dx = vorigin[0] - center[0];
+		if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
+		btScalar Dy = vorigin[1] - center[1];
+		if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
+		btScalar Dz = vorigin[2] - center[2];
+		if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;
+
+
+		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
+		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
+		f = vdir[2] * Dx - vdir[0] * Dz;
+		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
+		f = vdir[0] * Dy - vdir[1] * Dx;
+		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
+		return true;
+	}
+
+
+	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
+	{
+		btVector3 center = (m_max+m_min)*0.5f;
+		btVector3 extend = m_max-center;
+
+		btScalar _fOrigin =  direction.dot(center);
+		btScalar _fMaximumExtent = extend.dot(direction.absolute());
+		vmin = _fOrigin - _fMaximumExtent;
+		vmax = _fOrigin + _fMaximumExtent;
+	}
+
+	SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
+	{
+		btScalar _fmin,_fmax;
+		this->projection_interval(plane,_fmin,_fmax);
+
+		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
+		{
+			return G_BACK_PLANE; // 0
+		}
+
+		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
+		{
+			return G_COLLIDE_PLANE; //1
+		}
+		return G_FRONT_PLANE;//2
+	}
+
+	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
+	{
+		GIM_AABB tbox = box;
+		tbox.appy_transform(trans1_to_0);
+		return has_collision(tbox);
+	}
+
+	//! transcache is the transformation cache from box to this AABB
+	SIMD_FORCE_INLINE bool overlapping_trans_cache(
+		const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
+	{
+
+		//Taken from OPCODE
+		btVector3 ea,eb;//extends
+		btVector3 ca,cb;//extends
+		get_center_extend(ca,ea);
+		box.get_center_extend(cb,eb);
+
+
+		btVector3 T;
+		btScalar t,t2;
+		int i;
+
+		// Class I : A's basis vectors
+		for(i=0;i<3;i++)
+		{
+			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
+			t = transcache.m_AR[i].dot(eb) + ea[i];
+			if(GIM_GREATER(T[i], t))	return false;
+		}
+		// Class II : B's basis vectors
+		for(i=0;i<3;i++)
+		{
+			t = MAT_DOT_COL(transcache.m_R1to0,T,i);
+			t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
+			if(GIM_GREATER(t,t2))	return false;
+		}
+		// Class III : 9 cross products
+		if(fulltest)
+		{
+			int j,m,n,o,p,q,r;
+			for(i=0;i<3;i++)
+			{
+				m = (i+1)%3;
+				n = (i+2)%3;
+				o = i==0?1:0;
+				p = i==2?1:2;
+				for(j=0;j<3;j++)
+				{
+					q = j==2?1:2;
+					r = j==0?1:0;
+					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
+					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
+						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
+					if(GIM_GREATER(t,t2))	return false;
+				}
+			}
+		}
+		return true;
+	}
+
+	//! Simple test for planes.
+	SIMD_FORCE_INLINE bool collide_plane(
+		const btVector4 & plane)
+	{
+		ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
+		return (classify == G_COLLIDE_PLANE);
+	}
+
+	//! test for a triangle, with edges
+	SIMD_FORCE_INLINE bool collide_triangle_exact(
+		const btVector3 & p1,
+		const btVector3 & p2,
+		const btVector3 & p3,
+		const btVector4 & triangle_plane)
+	{
+		if(!collide_plane(triangle_plane)) return false;
+
+		btVector3 center,extends;
+		this->get_center_extend(center,extends);
+
+		const btVector3 v1(p1 - center);
+		const btVector3 v2(p2 - center);
+		const btVector3 v3(p3 - center);
+
+		//First axis
+		btVector3 diff(v2 - v1);
+		btVector3 abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
+
+
+		diff = v3 - v2;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
+
+		diff = v1 - v3;
+		abs_diff = diff.absolute();
+		//Test With X axis
+		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Y axis
+		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+		//Test With Z axis
+		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
+
+		return true;
+	}
+};
+
+
+//! Compairison of transformation objects
+SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
+{
+	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
+
+	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
+	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
+	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
+	return true;
+}
+
+
+//! @}
+
+#endif // GIM_BOX_COLLISION_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp	(revision 1963)
@@ -0,0 +1,182 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_box_set.h"
+
+
+GUINT GIM_BOX_TREE::_calc_splitting_axis(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT i;
+
+	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+	GUINT numIndices = endIndex-startIndex;
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		means+=center;
+	}
+	means *= (btScalar(1.)/(btScalar)numIndices);
+
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+					 primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center-means;
+		diff2 = diff2 * diff2;
+		variance += diff2;
+	}
+	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+
+	return variance.maxAxis();
+}
+
+
+GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
+	gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
+	GUINT endIndex, GUINT splitAxis)
+{
+	GUINT i;
+	GUINT splitIndex =startIndex;
+	GUINT numIndices = endIndex - startIndex;
+
+	// average of centers
+	btScalar splitValue = 0.0f;
+	for (i=startIndex;i<endIndex;i++)
+	{
+		splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+	}
+	splitValue /= (btScalar)numIndices;
+
+	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+	for (i=startIndex;i<endIndex;i++)
+	{
+		btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
+					 primitive_boxes[i].m_bound.m_min[splitAxis]);
+		if (center > splitValue)
+		{
+			//swap
+			primitive_boxes.swap(i,splitIndex);
+			splitIndex++;
+		}
+	}
+
+	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+	//otherwise the tree-building might fail due to stack-overflows in certain cases.
+	//unbalanced1 is unsafe: it can cause stack overflows
+	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+	//unbalanced2 should work too: always use center (perfect balanced trees)
+	//bool unbalanced2 = true;
+
+	//this should be safe too:
+	GUINT rangeBalancedIndices = numIndices/3;
+	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+	if (unbalanced)
+	{
+		splitIndex = startIndex+ (numIndices>>1);
+	}
+
+	bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
+	btAssert(!unbal);
+
+	return splitIndex;
+}
+
+
+void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex)
+{
+	GUINT current_index = m_num_nodes++;
+
+	btAssert((endIndex-startIndex)>0);
+
+	if((endIndex-startIndex) == 1) //we got a leaf
+	{		
+		m_node_array[current_index].m_left = 0;
+		m_node_array[current_index].m_right = 0;
+		m_node_array[current_index].m_escapeIndex = 0;
+
+		m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound;
+		m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data;
+		return;
+	}
+
+	//configure inner node
+
+	GUINT splitIndex;
+
+	//calc this node bounding box
+	m_node_array[current_index].m_bound.invalidate();	
+	for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
+	{
+		m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
+	}
+
+	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+	//split axis
+	splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+	splitIndex = _sort_and_calc_splitting_index(
+			primitive_boxes,startIndex,endIndex,splitIndex);
+
+	//configure this inner node : the left node index
+	m_node_array[current_index].m_left = m_num_nodes;
+	//build left child tree
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+	//configure this inner node : the right node index
+	m_node_array[current_index].m_right = m_num_nodes;
+
+	//build right child tree
+	_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+	//configure this inner node : the escape index
+	m_node_array[current_index].m_escapeIndex  = m_num_nodes - current_index;
+}
+
+//! stackless build tree
+void GIM_BOX_TREE::build_tree(
+	gim_array<GIM_AABB_DATA> & primitive_boxes)
+{
+	// initialize node count to 0
+	m_num_nodes = 0;
+	// allocate nodes
+	m_node_array.resize(primitive_boxes.size()*2);
+	
+	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_box_set.h	(revision 1963)
@@ -0,0 +1,678 @@
+#ifndef GIM_BOX_SET_H_INCLUDED
+#define GIM_BOX_SET_H_INCLUDED
+
+/*! \file gim_box_set.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_array.h"
+#include "gim_radixsort.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+
+/*! \defgroup BOX_PRUNNING
+
+
+
+*/
+//! @{
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+    GUINT m_index1;
+    GUINT m_index2;
+    GIM_PAIR()
+    {}
+
+    GIM_PAIR(const GIM_PAIR & p)
+    {
+    	m_index1 = p.m_index1;
+    	m_index2 = p.m_index2;
+	}
+
+	GIM_PAIR(GUINT index1, GUINT index2)
+    {
+    	m_index1 = index1;
+    	m_index2 = index2;
+	}
+};
+
+//! A pairset array
+class gim_pair_set: public gim_array<GIM_PAIR>
+{
+public:
+	gim_pair_set():gim_array<GIM_PAIR>(32)
+	{
+	}
+	inline void push_pair(GUINT index1,GUINT index2)
+	{
+		push_back(GIM_PAIR(index1,index2));
+	}
+
+	inline void push_pair_inv(GUINT index1,GUINT index2)
+	{
+		push_back(GIM_PAIR(index2,index1));
+	}
+};
+
+
+//! Prototype Base class for primitive classification
+/*!
+This class is a wrapper for primitive collections.
+This tells relevant info for the Bounding Box set classes, which take care of space classification.
+This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
+*/
+class GIM_PRIMITIVE_MANAGER_PROTOTYPE
+{
+public:
+
+	//! determines if this manager consist on only triangles, which special case will be optimized
+	virtual bool is_trimesh() = 0;
+	virtual GUINT get_primitive_count() = 0;
+	virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
+	virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
+};
+
+
+struct GIM_AABB_DATA
+{
+	GIM_AABB m_bound;
+	GUINT m_data;
+};
+
+//! Node Structure for trees
+struct GIM_BOX_TREE_NODE
+{
+	GIM_AABB m_bound;
+	GUINT m_left;//!< Left subtree
+	GUINT m_right;//!< Right subtree
+	GUINT m_escapeIndex;//!< Scape index for traversing
+	GUINT m_data;//!< primitive index if apply
+
+	GIM_BOX_TREE_NODE()
+	{
+	    m_left = 0;
+	    m_right = 0;
+	    m_escapeIndex = 0;
+	    m_data = 0;
+	}
+
+	SIMD_FORCE_INLINE bool is_leaf_node() const
+	{
+	    return  (!m_left && !m_right);
+	}
+};
+
+//! Basic Box tree structure
+class GIM_BOX_TREE
+{
+protected:
+	GUINT m_num_nodes;
+	gim_array<GIM_BOX_TREE_NODE> m_node_array;
+protected:
+	GUINT _sort_and_calc_splitting_index(
+		gim_array<GIM_AABB_DATA> & primitive_boxes,
+		 GUINT startIndex,  GUINT endIndex, GUINT splitAxis);
+
+	GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
+
+	void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
+public:
+	GIM_BOX_TREE()
+	{
+		m_num_nodes = 0;
+	}
+
+	//! prototype functions for box tree management
+	//!@{
+	void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);
+
+	SIMD_FORCE_INLINE void clearNodes()
+	{
+		m_node_array.clear();
+		m_num_nodes = 0;
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE GUINT getNodeCount() const
+	{
+		return m_num_nodes;
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].is_leaf_node();
+	}
+
+	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].m_data;
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
+	{
+		bound = m_node_array[nodeindex].m_bound;
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	{
+		m_node_array[nodeindex].m_bound = bound;
+	}
+
+	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex)  const
+	{
+		return m_node_array[nodeindex].m_left;
+	}
+
+	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex)  const
+	{
+		return m_node_array[nodeindex].m_right;
+	}
+
+	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+	{
+		return m_node_array[nodeindex].m_escapeIndex;
+	}
+
+	//!@}
+};
+
+
+//! Generic Box Tree Template
+/*!
+This class offers an structure for managing a box tree of primitives.
+Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
+a Box tree structure ( like GIM_BOX_TREE).
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
+class GIM_BOX_TREE_TEMPLATE_SET
+{
+protected:
+	_GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
+	_GIM_BOX_TREE_PROTOTYPE m_box_tree;
+protected:
+	//stackless refit
+	SIMD_FORCE_INLINE void refit()
+	{
+		GUINT nodecount = getNodeCount();
+		while(nodecount--)
+		{
+			if(isLeafNode(nodecount))
+			{
+				GIM_AABB leafbox;
+				m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
+				setNodeBound(nodecount,leafbox);
+			}
+			else
+			{
+				//get left bound
+				GUINT childindex = getLeftNodeIndex(nodecount);
+				GIM_AABB bound;
+				getNodeBound(childindex,bound);
+				//get right bound
+				childindex = getRightNodeIndex(nodecount);
+				GIM_AABB bound2;
+				getNodeBound(childindex,bound2);
+				bound.merge(bound2);
+
+				setNodeBound(nodecount,bound);
+			}
+		}
+	}
+public:
+
+	GIM_BOX_TREE_TEMPLATE_SET()
+	{
+	}
+
+	SIMD_FORCE_INLINE GIM_AABB getGlobalBox()  const
+	{
+		GIM_AABB totalbox;
+		getNodeBound(0, totalbox);
+		return totalbox;
+	}
+
+	SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
+	{
+		m_primitive_manager = primitive_manager;
+	}
+
+	const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
+	{
+		return m_primitive_manager;
+	}
+
+	_GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
+	{
+		return m_primitive_manager;
+	}
+
+//! node manager prototype functions
+///@{
+
+	//! this attemps to refit the box set.
+	SIMD_FORCE_INLINE void update()
+	{
+		refit();
+	}
+
+	//! this rebuild the entire set
+	SIMD_FORCE_INLINE void buildSet()
+	{
+		//obtain primitive boxes
+		gim_array<GIM_AABB_DATA> primitive_boxes;
+		primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);
+
+		for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
+		{
+			 m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
+			 primitive_boxes[i].m_data = i;
+		}
+
+		m_box_tree.build_tree(primitive_boxes);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
+	{
+		GUINT curIndex = 0;
+		GUINT numNodes = getNodeCount();
+
+		while (curIndex < numNodes)
+		{
+			GIM_AABB bound;
+			getNodeBound(curIndex,bound);
+
+			//catch bugs in tree data
+
+			bool aabbOverlap = bound.has_collision(box);
+			bool isleafnode = isLeafNode(curIndex);
+
+			if (isleafnode && aabbOverlap)
+			{
+				collided_results.push_back(getNodeData(curIndex));
+			}
+
+			if (aabbOverlap || isleafnode)
+			{
+				//next subnode
+				curIndex++;
+			}
+			else
+			{
+				//skip node
+				curIndex+= getScapeNodeIndex(curIndex);
+			}
+		}
+		if(collided_results.size()>0) return true;
+		return false;
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
+		 const btTransform & transform, gim_array<GUINT> & collided_results) const
+	{
+		GIM_AABB transbox=box;
+		transbox.appy_transform(transform);
+		return boxQuery(transbox,collided_results);
+	}
+
+	//! returns the indices of the primitives in the m_primitive_manager
+	SIMD_FORCE_INLINE bool rayQuery(
+		const btVector3 & ray_dir,const btVector3 & ray_origin ,
+		gim_array<GUINT> & collided_results) const
+	{
+		GUINT curIndex = 0;
+		GUINT numNodes = getNodeCount();
+
+		while (curIndex < numNodes)
+		{
+			GIM_AABB bound;
+			getNodeBound(curIndex,bound);
+
+			//catch bugs in tree data
+
+			bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+			bool isleafnode = isLeafNode(curIndex);
+
+			if (isleafnode && aabbOverlap)
+			{
+				collided_results.push_back(getNodeData( curIndex));
+			}
+
+			if (aabbOverlap || isleafnode)
+			{
+				//next subnode
+				curIndex++;
+			}
+			else
+			{
+				//skip node
+				curIndex+= getScapeNodeIndex(curIndex);
+			}
+		}
+		if(collided_results.size()>0) return true;
+		return false;
+	}
+
+	//! tells if this set has hierarcht
+	SIMD_FORCE_INLINE bool hasHierarchy() const
+	{
+		return true;
+	}
+
+	//! tells if this set is a trimesh
+	SIMD_FORCE_INLINE bool isTrimesh()  const
+	{
+		return m_primitive_manager.is_trimesh();
+	}
+
+	//! node count
+	SIMD_FORCE_INLINE GUINT getNodeCount() const
+	{
+		return m_box_tree.getNodeCount();
+	}
+
+	//! tells if the node is a leaf
+	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
+	{
+		return m_box_tree.isLeafNode(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
+	{
+		return m_box_tree.getNodeData(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound)  const
+	{
+		m_box_tree.getNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
+	{
+		m_box_tree.setNodeBound(nodeindex, bound);
+	}
+
+	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getLeftNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getRightNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
+	{
+		return m_box_tree.getScapeNodeIndex(nodeindex);
+	}
+
+	SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
+	{
+		m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
+	}
+
+//! @}
+};
+
+//! Class for Box Tree Sets
+/*!
+this has the GIM_BOX_TREE implementation for bounding boxes.
+*/
+template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
+class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
+{
+public:
+
+};
+
+
+
+
+
+/// GIM_BOX_SET collision methods
+template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
+class GIM_TREE_TREE_COLLIDER
+{
+public:
+	gim_pair_set * m_collision_pairs;
+	BOX_SET_CLASS0 * m_boxset0;
+	BOX_SET_CLASS1 * m_boxset1;
+	GUINT current_node0;
+	GUINT current_node1;
+	bool node0_is_leaf;
+	bool node1_is_leaf;
+	bool t0_is_trimesh;
+	bool t1_is_trimesh;
+	bool node0_has_triangle;
+	bool node1_has_triangle;
+	GIM_AABB m_box0;
+	GIM_AABB m_box1;
+	GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+	btTransform trans_cache_0to1;
+	GIM_TRIANGLE m_tri0;
+	btVector4 m_tri0_plane;
+	GIM_TRIANGLE m_tri1;
+	btVector4 m_tri1_plane;
+
+
+public:
+	GIM_TREE_TREE_COLLIDER()
+	{
+		current_node0 = G_UINT_INFINITY;
+		current_node1 = G_UINT_INFINITY;
+	}
+protected:
+	SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
+	{
+		if(node0_has_triangle) return;
+		m_boxset0->getNodeTriangle(node0,m_tri0);
+		//transform triangle
+		m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
+		m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
+		m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]);
+		m_tri0.get_plane(m_tri0_plane);
+
+		node0_has_triangle = true;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
+	{
+		if(node1_has_triangle) return;
+		m_boxset1->getNodeTriangle(node1,m_tri1);
+		//transform triangle
+		m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
+		m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
+		m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]);
+		m_tri1.get_plane(m_tri1_plane);
+
+		node1_has_triangle = true;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
+	{
+		if(node0 == current_node0) return;
+		m_boxset0->getNodeBound(node0,m_box0);
+		node0_is_leaf = m_boxset0->isLeafNode(node0);
+		node0_has_triangle = false;
+		current_node0 = node0;
+	}
+
+	SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
+	{
+		if(node1 == current_node1) return;
+		m_boxset1->getNodeBound(node1,m_box1);
+		node1_is_leaf = m_boxset1->isLeafNode(node1);
+		node1_has_triangle = false;
+		current_node1 = node1;
+	}
+
+	SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
+	{
+		retrieve_node0_info(node0);
+		retrieve_node1_info(node1);
+		bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
+		if(!result) return false;
+
+		if(t0_is_trimesh && node0_is_leaf)
+		{
+			//perform primitive vs box collision
+			retrieve_node0_triangle(node0);
+			//do triangle vs box collision
+			m_box1.increment_margin(m_tri0.m_margin);
+
+			result = m_box1.collide_triangle_exact(
+				m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);
+
+			m_box1.increment_margin(-m_tri0.m_margin);
+
+			if(!result) return false;
+			return true;
+		}
+		else if(t1_is_trimesh && node1_is_leaf)
+		{
+			//perform primitive vs box collision
+			retrieve_node1_triangle(node1);
+			//do triangle vs box collision
+			m_box0.increment_margin(m_tri1.m_margin);
+
+			result = m_box0.collide_triangle_exact(
+				m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);
+
+			m_box0.increment_margin(-m_tri1.m_margin);
+
+			if(!result) return false;
+			return true;
+		}
+		return true;
+	}
+
+	//stackless collision routine
+	void find_collision_pairs()
+	{
+		gim_pair_set stack_collisions;
+		stack_collisions.reserve(32);
+
+		//add the first pair
+		stack_collisions.push_pair(0,0);
+
+
+		while(stack_collisions.size())
+		{
+			//retrieve the last pair and pop
+			GUINT node0 = stack_collisions.back().m_index1;
+			GUINT node1 = stack_collisions.back().m_index2;
+			stack_collisions.pop_back();
+			if(node_collision(node0,node1)) // a collision is found
+			{
+				if(node0_is_leaf)
+				{
+					if(node1_is_leaf)
+					{
+						m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
+					}
+					else
+					{
+						//collide left
+						stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));
+
+						//collide right
+						stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
+					}
+				}
+				else
+				{
+					if(node1_is_leaf)
+					{
+						//collide left
+						stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
+						//collide right
+						stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
+					}
+					else
+					{
+						GUINT left0 = m_boxset0->getLeftNodeIndex(node0);
+						GUINT right0 = m_boxset0->getRightNodeIndex(node0);
+						GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
+						GUINT right1 = m_boxset1->getRightNodeIndex(node1);
+						//collide left
+						stack_collisions.push_pair(left0,left1);
+						//collide right
+						stack_collisions.push_pair(left0,right1);
+						//collide left
+						stack_collisions.push_pair(right0,left1);
+						//collide right
+						stack_collisions.push_pair(right0,right1);
+
+					}// else if node1 is not a leaf
+				}// else if node0 is not a leaf
+
+			}// if(node_collision(node0,node1))
+		}//while(stack_collisions.size())
+	}
+public:
+	void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
+		BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
+		gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
+	{
+		m_collision_pairs = &collision_pairs;
+		m_boxset0 = boxset1;
+		m_boxset1 = boxset2;
+
+		trans_cache_1to0.calc_from_homogenic(trans1,trans2);
+
+		trans_cache_0to1 =  trans2.inverse();
+		trans_cache_0to1 *= trans1;
+
+
+		if(complete_primitive_tests)
+		{
+			t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
+			t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
+		}
+		else
+		{
+			t0_is_trimesh = false;
+			t1_is_trimesh = false;
+		}
+
+		find_collision_pairs();
+	}
+};
+
+
+#endif // GIM_BOXPRUNING_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_clip_polygon.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_clip_polygon.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_clip_polygon.h	(revision 1963)
@@ -0,0 +1,214 @@
+#ifndef GIM_CLIP_POLYGON_H_INCLUDED
+#define GIM_CLIP_POLYGON_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+//! This function calcs the distance from a 3D plane
+class DISTANCE_PLANE_3D_FUNC
+{
+public:
+	template<typename CLASS_POINT,typename CLASS_PLANE>
+	inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
+	{
+		return DISTANCE_PLANE_POINT(plane, point);
+	}
+};
+
+
+
+template<typename CLASS_POINT>
+SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						GREAL dist0,
+						GREAL dist1,
+						CLASS_POINT * clipped,
+						GUINT & clipped_count)
+{
+	GUINT _prevclassif = (dist0>G_EPSILON);
+	GUINT _classif = (dist1>G_EPSILON);
+	if(_classif!=_prevclassif)
+	{
+		GREAL blendfactor = -dist0/(dist1-dist0);
+		VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
+		clipped_count++;
+	}
+	if(!_classif)
+	{
+		VEC_COPY(clipped[clipped_count],point1);
+		clipped_count++;
+	}
+}
+
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT * polygon_points,
+						GUINT polygon_point_count,
+						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+    GUINT clipped_count = 0;
+
+
+    //clip first point
+	GREAL firstdist = distance_func(plane,polygon_points[0]);;
+	if(!(firstdist>G_EPSILON))
+	{
+		VEC_COPY(clipped[clipped_count],polygon_points[0]);
+		clipped_count++;
+	}
+
+	GREAL olddist = firstdist;
+	for(GUINT _i=1;_i<polygon_point_count;_i++)
+	{		
+		GREAL dist = distance_func(plane,polygon_points[_i]);
+
+		PLANE_CLIP_POLYGON_COLLECT(
+						polygon_points[_i-1],polygon_points[_i],
+						olddist,
+						dist,
+						clipped,
+						clipped_count);
+
+
+		olddist = dist;		
+	}
+
+	//RETURN TO FIRST  point	
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					polygon_points[polygon_point_count-1],polygon_points[0],
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+//! Clips a polygon by a plane
+/*!
+*\return The count of the clipped counts
+*/
+template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						const CLASS_POINT & point2,
+						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
+{
+    GUINT clipped_count = 0;
+
+    //clip first point
+	GREAL firstdist = distance_func(plane,point0);;
+	if(!(firstdist>G_EPSILON))
+	{
+		VEC_COPY(clipped[clipped_count],point0);
+		clipped_count++;
+	}
+
+	// point 1
+	GREAL olddist = firstdist;
+	GREAL dist = distance_func(plane,point1);
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					point0,point1,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+
+	olddist = dist;
+
+
+	// point 2
+	dist = distance_func(plane,point2);
+
+	PLANE_CLIP_POLYGON_COLLECT(
+					point1,point2,
+					olddist,
+					dist,
+					clipped,
+					clipped_count);
+	olddist = dist;
+
+
+
+	//RETURN TO FIRST  point
+	PLANE_CLIP_POLYGON_COLLECT(
+					point2,point0,
+					olddist,
+					firstdist,
+					clipped,
+					clipped_count);
+
+	return clipped_count;
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT * polygon_points,
+						GUINT polygon_point_count,
+						CLASS_POINT * clipped)
+{
+	return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+template<typename CLASS_POINT,typename CLASS_PLANE>
+SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
+						const CLASS_PLANE & plane,
+						const CLASS_POINT & point0,
+						const CLASS_POINT & point1,
+						const CLASS_POINT & point2,
+						CLASS_POINT * clipped)
+{
+	return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
+}
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.cpp	(revision 1963)
@@ -0,0 +1,146 @@
+
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_contact.h"
+
+#define MAX_COINCIDENT 8
+
+void gim_contact_array::merge_contacts(
+	const gim_contact_array & contacts, bool normal_contact_average)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
+	keycontacts.resize(contacts.size(),false);
+
+	//fill key contacts
+
+	GUINT i;
+
+	for (i = 0;i<contacts.size() ;i++ )
+	{
+		keycontacts[i].m_key = contacts[i].calc_key_contact();
+		keycontacts[i].m_value = i;
+	}
+
+	//sort keys
+	gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
+
+	// Merge contacts
+
+	GUINT coincident_count=0;
+	btVector3 coincident_normals[MAX_COINCIDENT];
+
+	GUINT last_key = keycontacts[0].m_key;
+	GUINT key = 0;
+
+	push_back(contacts[keycontacts[0].m_value]);
+	GIM_CONTACT * pcontact = &back();
+
+
+
+	for( i=1;i<keycontacts.size();i++)
+	{
+	    key = keycontacts[i].m_key;
+		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
+
+		if(last_key ==  key)//same points
+		{
+			//merge contact
+			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
+			{
+				*pcontact = *scontact;
+                coincident_count = 0;
+			}
+			else if(normal_contact_average)
+			{
+				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
+                {
+                    if(coincident_count<MAX_COINCIDENT)
+                    {
+                    	coincident_normals[coincident_count] = scontact->m_normal;
+                        coincident_count++;
+                    }
+                }
+			}
+		}
+		else
+		{//add new contact
+
+		    if(normal_contact_average && coincident_count>0)
+		    {
+		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
+		        coincident_count = 0;
+		    }
+
+		    push_back(*scontact);
+		    pcontact = &back();
+        }
+		last_key = key;
+	}
+}
+
+void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
+{
+	clear();
+
+	if(contacts.size()==1)
+	{
+		push_back(contacts.back());
+		return;
+	}
+
+	GIM_CONTACT average_contact = contacts.back();
+
+	for (GUINT i=1;i<contacts.size() ;i++ )
+	{
+		average_contact.m_point += contacts[i].m_point;
+		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
+	}
+
+	//divide
+	GREAL divide_average = 1.0f/((GREAL)contacts.size());
+
+	average_contact.m_point *= divide_average;
+
+	average_contact.m_normal *= divide_average;
+
+	average_contact.m_depth = average_contact.m_normal.length();
+
+	average_contact.m_normal /= average_contact.m_depth;
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_contact.h	(revision 1963)
@@ -0,0 +1,172 @@
+#ifndef GIM_CONTACT_H_INCLUDED
+#define GIM_CONTACT_H_INCLUDED
+
+/*! \file gim_contact.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+#include "gim_geometry.h"
+#include "gim_radixsort.h"
+#include "gim_array.h"
+
+/*! \defgroup CONTACTS
+\brief
+Functions for managing and sorting contacts resulting from a collision query.
+<ul>
+<li> Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST
+<li> After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY
+<li> Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts
+</ul>
+
+*/
+//! @{
+
+/**
+Configuration var for applying interpolation of  contact normals
+*/
+#define NORMAL_CONTACT_AVERAGE 1
+#define CONTACT_DIFF_EPSILON 0.00001f
+
+/// Structure for collision results
+class GIM_CONTACT
+{
+public:
+    btVector3 m_point;
+    btVector3 m_normal;
+    GREAL m_depth;//Positive value indicates interpenetration
+    GREAL m_distance;//Padding not for use
+    GUINT m_feature1;//Face number
+    GUINT m_feature2;//Face number
+public:
+    GIM_CONTACT()
+    {
+    }
+
+    GIM_CONTACT(const GIM_CONTACT & contact):
+				m_point(contact.m_point),
+				m_normal(contact.m_normal),
+				m_depth(contact.m_depth),
+				m_feature1(contact.m_feature1),
+				m_feature2(contact.m_feature2)
+    {
+    	m_point = contact.m_point;
+    	m_normal = contact.m_normal;
+    	m_depth = contact.m_depth;
+    	m_feature1 = contact.m_feature1;
+    	m_feature2 = contact.m_feature2;
+    }
+
+    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
+    	 			GREAL depth, GUINT feature1, GUINT feature2):
+				m_point(point),
+				m_normal(normal),
+				m_depth(depth),
+				m_feature1(feature1),
+				m_feature2(feature2)
+    {
+    }
+
+	//! Calcs key for coord classification
+    SIMD_FORCE_INLINE GUINT calc_key_contact() const
+    {
+    	GINT _coords[] = {
+    		(GINT)(m_point[0]*1000.0f+1.0f),
+    		(GINT)(m_point[1]*1333.0f),
+    		(GINT)(m_point[2]*2133.0f+3.0f)};
+		GUINT _hash=0;
+		GUINT *_uitmp = (GUINT *)(&_coords[0]);
+		_hash = *_uitmp;
+		_uitmp++;
+		_hash += (*_uitmp)<<4;
+		_uitmp++;
+		_hash += (*_uitmp)<<8;
+		return _hash;
+    }
+
+    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
+    {
+    	btVector3 vec_sum(m_normal);
+		for(GUINT i=0;i<normal_count;i++)
+		{
+			vec_sum += normals[i];
+		}
+
+		GREAL vec_sum_len = vec_sum.length2();
+		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
+
+		GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
+
+		m_normal = vec_sum*vec_sum_len;
+    }
+
+};
+
+
+class gim_contact_array:public gim_array<GIM_CONTACT>
+{
+public:
+	gim_contact_array():gim_array<GIM_CONTACT>(64)
+	{
+	}
+
+	SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
+    	 			GREAL depth, GUINT feature1, GUINT feature2)
+	{
+		push_back_mem();
+		GIM_CONTACT & newele = back();
+		newele.m_point = point;
+		newele.m_normal = normal;
+		newele.m_depth = depth;
+		newele.m_feature1 = feature1;
+		newele.m_feature2 = feature2;
+	}
+
+	SIMD_FORCE_INLINE void push_triangle_contacts(
+		const GIM_TRIANGLE_CONTACT_DATA & tricontact,
+		GUINT feature1,GUINT feature2)
+	{
+		for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
+		{
+			push_back_mem();
+			GIM_CONTACT & newele = back();
+			newele.m_point = tricontact.m_points[i];
+			newele.m_normal = tricontact.m_separating_normal;
+			newele.m_depth = tricontact.m_penetration_depth;
+			newele.m_feature1 = feature1;
+			newele.m_feature2 = feature2;
+		}
+	}
+
+	void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
+	void merge_contacts_unique(const gim_contact_array & contacts);
+};
+
+//! @}
+#endif // GIM_CONTACT_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geom_types.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geom_types.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geom_types.h	(revision 1963)
@@ -0,0 +1,102 @@
+#ifndef GIM_GEOM_TYPES_H_INCLUDED
+#define GIM_GEOM_TYPES_H_INCLUDED
+
+/*! \file gim_geom_types.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+
+/*! \defgroup GEOMETRIC_TYPES
+\brief
+Basic types and constants for geometry
+*/
+//! @{
+
+//! Short Integer vector 2D
+typedef GSHORT vec2s[2];
+//! Integer vector 3D
+typedef GSHORT vec3s[3];
+//! Integer vector 4D
+typedef GSHORT vec4s[4];
+
+//! Short Integer vector 2D
+typedef GUSHORT vec2us[2];
+//! Integer vector 3D
+typedef GUSHORT vec3us[3];
+//! Integer vector 4D
+typedef GUSHORT vec4us[4];
+
+//! Integer vector 2D
+typedef GINT vec2i[2];
+//! Integer vector 3D
+typedef GINT vec3i[3];
+//! Integer vector 4D
+typedef GINT vec4i[4];
+
+//! Unsigned Integer vector 2D
+typedef GUINT vec2ui[2];
+//! Unsigned Integer vector 3D
+typedef GUINT vec3ui[3];
+//! Unsigned Integer vector 4D
+typedef GUINT vec4ui[4];
+
+//! Float vector 2D
+typedef GREAL vec2f[2];
+//! Float vector 3D
+typedef GREAL vec3f[3];
+//! Float vector 4D
+typedef GREAL vec4f[4];
+
+//! Double vector 2D
+typedef GREAL2 vec2d[2];
+//! Float vector 3D
+typedef GREAL2 vec3d[3];
+//! Float vector 4D
+typedef GREAL2 vec4d[4];
+
+//! Matrix 2D, row ordered
+typedef GREAL mat2f[2][2];
+//! Matrix 3D, row ordered
+typedef GREAL mat3f[3][3];
+//! Matrix 4D, row ordered
+typedef GREAL mat4f[4][4];
+
+//! Quaternion
+typedef GREAL quatf[4];
+
+//typedef struct _aabb3f aabb3f;
+//! @}
+
+
+
+#endif // GIM_GEOM_TYPES_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geometry.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geometry.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_geometry.h	(revision 1963)
@@ -0,0 +1,42 @@
+#ifndef GIM_GEOMETRY_H_INCLUDED
+#define GIM_GEOMETRY_H_INCLUDED
+
+/*! \file gim_geometry.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+///Additional Headers for Collision
+#include "gim_basic_geometry_operations.h"
+#include "gim_clip_polygon.h"
+#include "gim_box_collision.h"
+#include "gim_tri_collision.h"
+
+#endif // GIM_VECTOR_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_hash_table.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_hash_table.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_hash_table.h	(revision 1963)
@@ -0,0 +1,920 @@
+#ifndef GIM_HASH_TABLE_H_INCLUDED
+#define GIM_HASH_TABLE_H_INCLUDED
+/*! \file gim_trimesh_data.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_radixsort.h"
+
+/*! \addtogroup CONTAINERS
+\brief
+Abstract class for collision objects
+*/
+//! @{
+
+#define GIM_INVALID_HASH 0xffffffff //!< A very very high value
+#define GIM_DEFAULT_HASH_TABLE_SIZE 380
+#define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4
+#define GIM_HASH_TABLE_GROW_FACTOR 2
+
+#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII
+
+template<typename T>
+struct GIM_HASH_TABLE_NODE
+{
+    GUINT m_key;
+    T m_data;
+    GIM_HASH_TABLE_NODE()
+    {
+    }
+
+    GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value)
+    {
+        m_key = value.m_key;
+        m_data = value.m_data;
+    }
+
+    GIM_HASH_TABLE_NODE(GUINT key, const T & data)
+    {
+        m_key = key;
+        m_data = data;
+    }
+
+    bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key <  other.m_key) return true;
+		return false;
+	}
+
+	bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key >  other.m_key) return true;
+		return false;
+	}
+
+	bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const
+	{
+		///inverse order, further objects are first
+		if(m_key ==  other.m_key) return true;
+		return false;
+	}
+};
+
+///Macro for getting the key
+class GIM_HASH_NODE_GET_KEY
+{
+public:
+	template<class T>
+	inline GUINT operator()( const T& a)
+	{
+		return a.m_key;
+	}
+};
+
+
+
+///Macro for comparing the key and the element
+class GIM_HASH_NODE_CMP_KEY_MACRO
+{
+public:
+	template<class T>
+	inline int operator() ( const T& a, GUINT key)
+	{
+		return ((int)(a.m_key - key));
+	}
+};
+
+///Macro for comparing Hash nodes
+class GIM_HASH_NODE_CMP_MACRO
+{
+public:
+	template<class T>
+	inline int operator() ( const T& a, const T& b )
+	{
+		return ((int)(a.m_key - b.m_key));
+	}
+};
+
+
+
+
+
+//! Sorting for hash table
+/*!
+switch automatically between quicksort and radixsort
+*/
+template<typename T>
+void gim_sort_hash_node_array(T * array, GUINT array_count)
+{
+    if(array_count<GIM_MIN_RADIX_SORT_SIZE)
+    {
+    	gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO());
+    }
+    else
+    {
+    	memcopy_elements_func cmpfunc;
+    	gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc);
+    }
+}
+
+
+
+
+
+
+// Note: assumes long is at least 32 bits.
+#define GIM_NUM_PRIME 28
+
+static const GUINT gim_prime_list[GIM_NUM_PRIME] =
+{
+  53ul,         97ul,         193ul,       389ul,       769ul,
+  1543ul,       3079ul,       6151ul,      12289ul,     24593ul,
+  49157ul,      98317ul,      196613ul,    393241ul,    786433ul,
+  1572869ul,    3145739ul,    6291469ul,   12582917ul,  25165843ul,
+  50331653ul,   100663319ul,  201326611ul, 402653189ul, 805306457ul,
+  1610612741ul, 3221225473ul, 4294967291ul
+};
+
+inline GUINT gim_next_prime(GUINT number)
+{
+    //Find nearest upper prime
+    GUINT result_ind = 0;
+    gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind);
+
+    // inv: result_ind < 28
+    return gim_prime_list[result_ind];
+}
+
+
+
+//! A compact hash table implementation
+/*!
+A memory aligned compact hash table that coud be treated as an array.
+It could be a simple sorted array without the overhead of the hash key bucked, or could
+be a formely hash table with an array of keys.
+You can use switch_to_hashtable() and switch_to_sorted_array for saving space or increase speed.
+</br>
+
+<ul>
+<li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+<li> If node_size != 0, then this container becomes a hash table for ever
+</ul>
+
+*/
+template<class T>
+class gim_hash_table
+{
+protected:
+    typedef GIM_HASH_TABLE_NODE<T> _node_type;
+
+    //!The nodes
+    //array< _node_type, SuperAllocator<_node_type> > m_nodes;
+    gim_array< _node_type > m_nodes;
+    //SuperBufferedArray< _node_type > m_nodes;
+    bool m_sorted;
+
+    /*! \defgroup HASH_TABLE_STRUCTURES
+    \brief
+    Hash table data management. The hash table has the indices to the corresponding m_nodes array
+    */
+    //! @{
+
+    GUINT * m_hash_table;//!<
+    GUINT m_table_size;//!<
+    GUINT m_node_size;//!<
+    GUINT m_min_hash_table_size;
+
+
+
+    //! Returns the cell index
+    inline GUINT _find_cell(GUINT hashkey)
+    {
+        _node_type * nodesptr = m_nodes.pointer();
+        GUINT start_index = (hashkey%m_table_size)*m_node_size;
+        GUINT end_index = start_index + m_node_size;
+
+        while(start_index<end_index)
+        {
+            GUINT value = m_hash_table[start_index];
+            if(value != GIM_INVALID_HASH)
+            {
+                if(nodesptr[value].m_key == hashkey) return start_index;
+            }
+            start_index++;
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
+    inline GUINT _find_avaliable_cell(GUINT hashkey)
+    {
+        _node_type * nodesptr = m_nodes.pointer();
+        GUINT avaliable_index = GIM_INVALID_HASH;
+        GUINT start_index = (hashkey%m_table_size)*m_node_size;
+        GUINT end_index = start_index + m_node_size;
+
+        while(start_index<end_index)
+        {
+            GUINT value = m_hash_table[start_index];
+            if(value == GIM_INVALID_HASH)
+            {
+                if(avaliable_index==GIM_INVALID_HASH)
+                {
+                    avaliable_index = start_index;
+                }
+            }
+            else if(nodesptr[value].m_key == hashkey)
+            {
+                return start_index;
+            }
+            start_index++;
+        }
+        return avaliable_index;
+    }
+
+
+
+    //! reserves the memory for the hash table.
+    /*!
+    \pre hash table must be empty
+    \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH.
+    */
+    inline void _reserve_table_memory(GUINT newtablesize)
+    {
+        if(newtablesize==0) return;
+        if(m_node_size==0) return;
+
+        //Get a Prime size
+
+        m_table_size = gim_next_prime(newtablesize);
+
+        GUINT datasize = m_table_size*m_node_size;
+        //Alloc the data buffer
+        m_hash_table =  (GUINT *)gim_alloc(datasize*sizeof(GUINT));
+    }
+
+    inline void _invalidate_keys()
+    {
+        GUINT datasize = m_table_size*m_node_size;
+        for(GUINT i=0;i<datasize;i++)
+        {
+            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+        }
+    }
+
+    //! Clear all memory for the hash table
+    inline void _clear_table_memory()
+    {
+        if(m_hash_table==NULL) return;
+        gim_free(m_hash_table);
+        m_hash_table = NULL;
+        m_table_size = 0;
+    }
+
+    //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
+    inline void _rehash()
+    {
+        _invalidate_keys();
+
+        _node_type * nodesptr = m_nodes.pointer();
+        for(GUINT i=0;i<(GUINT)m_nodes.size();i++)
+        {
+            GUINT nodekey = nodesptr[i].m_key;
+            if(nodekey != GIM_INVALID_HASH)
+            {
+                //Search for the avaliable cell in buffer
+                GUINT index = _find_avaliable_cell(nodekey);
+
+
+				if(m_hash_table[index]!=GIM_INVALID_HASH)
+				{//The new index is alreade used... discard this new incomming object, repeated key
+				    btAssert(m_hash_table[index]==nodekey);
+					nodesptr[i].m_key = GIM_INVALID_HASH;
+				}
+				else
+				{
+					//;
+					//Assign the value for alloc
+					m_hash_table[index] = i;
+				}
+            }
+        }
+    }
+
+    //! Resize hash table indices
+    inline void _resize_table(GUINT newsize)
+    {
+        //Clear memory
+        _clear_table_memory();
+        //Alloc the data
+        _reserve_table_memory(newsize);
+        //Invalidate keys and rehash
+        _rehash();
+    }
+
+    //! Destroy hash table memory
+    inline void _destroy()
+    {
+        if(m_hash_table==NULL) return;
+        _clear_table_memory();
+    }
+
+    //! Finds an avaliable hash table cell, and resizes the table if there isn't space
+    inline GUINT _assign_hash_table_cell(GUINT hashkey)
+    {
+        GUINT cell_index = _find_avaliable_cell(hashkey);
+
+        if(cell_index==GIM_INVALID_HASH)
+        {
+            //rehashing
+            _resize_table(m_table_size+1);
+            GUINT cell_index = _find_avaliable_cell(hashkey);
+            btAssert(cell_index!=GIM_INVALID_HASH);
+        }
+        return cell_index;
+    }
+
+    //! erase by index in hash table
+    inline bool _erase_by_index_hash_table(GUINT index)
+    {
+        if(index >= m_nodes.size()) return false;
+        if(m_nodes[index].m_key != GIM_INVALID_HASH)
+        {
+            //Search for the avaliable cell in buffer
+            GUINT cell_index = _find_cell(m_nodes[index].m_key);
+
+            btAssert(cell_index!=GIM_INVALID_HASH);
+            btAssert(m_hash_table[cell_index]==index);
+
+            m_hash_table[cell_index] = GIM_INVALID_HASH;
+        }
+
+        return this->_erase_unsorted(index);
+    }
+
+    //! erase by key in hash table
+    inline bool _erase_hash_table(GUINT hashkey)
+    {
+        if(hashkey == GIM_INVALID_HASH) return false;
+
+        //Search for the avaliable cell in buffer
+        GUINT cell_index = _find_cell(hashkey);
+        if(cell_index ==GIM_INVALID_HASH) return false;
+
+        GUINT index = m_hash_table[cell_index];
+        m_hash_table[cell_index] = GIM_INVALID_HASH;
+
+        return this->_erase_unsorted(index);
+    }
+
+
+
+    //! insert an element in hash table
+    /*!
+    If the element exists, this won't insert the element
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT _insert_hash_table(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH)
+        {
+            //Insert anyway
+            _insert_unsorted(hashkey,value);
+            return GIM_INVALID_HASH;
+        }
+
+        GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+        GUINT value_key = m_hash_table[cell_index];
+
+        if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited
+
+        m_hash_table[cell_index] = m_nodes.size();
+
+        _insert_unsorted(hashkey,value);
+        return GIM_INVALID_HASH;
+    }
+
+    //! insert an element in hash table.
+    /*!
+    If the element exists, this replaces the element.
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH)
+        {
+            //Insert anyway
+            _insert_unsorted(hashkey,value);
+            return GIM_INVALID_HASH;
+        }
+
+        GUINT cell_index = _assign_hash_table_cell(hashkey);
+
+        GUINT value_key = m_hash_table[cell_index];
+
+        if(value_key!= GIM_INVALID_HASH)
+        {//replaces the existing
+            m_nodes[value_key] = _node_type(hashkey,value);
+            return value_key;// index of the replaced element
+        }
+
+        m_hash_table[cell_index] = m_nodes.size();
+
+        _insert_unsorted(hashkey,value);
+        return GIM_INVALID_HASH;
+
+    }
+
+    //! @}
+
+    /*! \defgroup SORTED_ARRAY_STRUCTURES
+    \brief
+    Sorted array data management. The hash table has the indices to the corresponding m_nodes array
+    */
+    //! @{
+    inline bool _erase_sorted(GUINT index)
+    {
+        if(index>=(GUINT)m_nodes.size()) return false;
+        m_nodes.erase_sorted(index);
+		if(m_nodes.size()<2) m_sorted = false;
+        return true;
+    }
+
+    //! faster, but unsorted
+    inline bool _erase_unsorted(GUINT index)
+    {
+        if(index>=m_nodes.size()) return false;
+
+        GUINT lastindex = m_nodes.size()-1;
+        if(index<lastindex && m_hash_table!=0)
+        {
+			GUINT hashkey =  m_nodes[lastindex].m_key;
+			if(hashkey!=GIM_INVALID_HASH)
+			{
+				//update the new position of the last element
+				GUINT cell_index = _find_cell(hashkey);
+				btAssert(cell_index!=GIM_INVALID_HASH);
+				//new position of the last element which will be swaped
+				m_hash_table[cell_index] = index;
+			}
+        }
+        m_nodes.erase(index);
+        m_sorted = false;
+        return true;
+    }
+
+    //! Insert in position ordered
+    /*!
+    Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable
+    */
+    inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos)
+    {
+        m_nodes.insert(_node_type(hashkey,value),pos);
+        this->check_for_switching_to_hashtable();
+    }
+
+    //! Insert an element in an ordered array
+    inline GUINT _insert_sorted(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH || size()==0)
+        {
+            m_nodes.push_back(_node_type(hashkey,value));
+            return GIM_INVALID_HASH;
+        }
+        //Insert at last position
+        //Sort element
+
+
+        GUINT result_ind=0;
+        GUINT last_index = m_nodes.size()-1;
+        _node_type * ptr = m_nodes.pointer();
+
+        bool found = gim_binary_search_ex(
+        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+        //Insert before found index
+        if(found)
+        {
+            return result_ind;
+        }
+        else
+        {
+            _insert_in_pos(hashkey, value, result_ind);
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value)
+    {
+        if(hashkey==GIM_INVALID_HASH || size()==0)
+        {
+            m_nodes.push_back(_node_type(hashkey,value));
+            return GIM_INVALID_HASH;
+        }
+        //Insert at last position
+        //Sort element
+        GUINT result_ind;
+        GUINT last_index = m_nodes.size()-1;
+        _node_type * ptr = m_nodes.pointer();
+
+        bool found = gim_binary_search_ex(
+        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+        //Insert before found index
+        if(found)
+        {
+            m_nodes[result_ind] = _node_type(hashkey,value);
+        }
+        else
+        {
+            _insert_in_pos(hashkey, value, result_ind);
+        }
+        return result_ind;
+    }
+
+    //! Fast insertion in m_nodes array
+    inline GUINT  _insert_unsorted(GUINT hashkey, const T & value)
+    {
+        m_nodes.push_back(_node_type(hashkey,value));
+        m_sorted = false;
+        return GIM_INVALID_HASH;
+    }
+
+    //! @}
+
+
+public:
+
+    /*!
+        <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
+        When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
+        <li> If node_size != 0, then this container becomes a hash table for ever
+        </ul>
+    */
+    gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
+                     GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
+                     GUINT min_hash_table_size = GIM_INVALID_HASH)
+    {
+        m_hash_table = NULL;
+        m_table_size = 0;
+        m_sorted = false;
+        m_node_size = node_size;
+        m_min_hash_table_size = min_hash_table_size;
+
+        if(m_node_size!=0)
+        {
+            if(reserve_size!=0)
+            {
+                m_nodes.reserve(reserve_size);
+                _reserve_table_memory(reserve_size);
+                _invalidate_keys();
+            }
+            else
+            {
+                m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
+                _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
+                _invalidate_keys();
+            }
+        }
+        else if(reserve_size!=0)
+        {
+            m_nodes.reserve(reserve_size);
+        }
+
+    }
+
+    ~gim_hash_table()
+    {
+        _destroy();
+    }
+
+    inline bool is_hash_table()
+    {
+        if(m_hash_table) return true;
+        return false;
+    }
+
+    inline bool is_sorted()
+    {
+        if(size()<2) return true;
+        return m_sorted;
+    }
+
+    bool sort()
+    {
+        if(is_sorted()) return true;
+        if(m_nodes.size()<2) return false;
+
+
+        _node_type * ptr = m_nodes.pointer();
+        GUINT siz = m_nodes.size();
+        gim_sort_hash_node_array(ptr,siz);
+        m_sorted=true;
+
+
+
+        if(m_hash_table)
+        {
+            _rehash();
+        }
+        return true;
+    }
+
+    bool switch_to_hashtable()
+    {
+        if(m_hash_table) return false;
+        if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+        if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE)
+        {
+            _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
+        }
+        else
+        {
+            _resize_table(m_nodes.size()+1);
+        }
+
+        return true;
+    }
+
+    bool switch_to_sorted_array()
+    {
+        if(m_hash_table==NULL) return true;
+        _clear_table_memory();
+        return sort();
+    }
+
+    //!If the container reaches the
+    bool check_for_switching_to_hashtable()
+    {
+        if(this->m_hash_table) return true;
+
+        if(!(m_nodes.size()< m_min_hash_table_size))
+        {
+            if(m_node_size == 0)
+            {
+                m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
+            }
+
+            _resize_table(m_nodes.size()+1);
+            return true;
+        }
+        return false;
+    }
+
+    inline void set_sorted(bool value)
+    {
+    	m_sorted = value;
+    }
+
+    //! Retrieves the amount of keys.
+    inline GUINT size() const
+    {
+        return m_nodes.size();
+    }
+
+    //! Retrieves the hash key.
+    inline GUINT get_key(GUINT index) const
+    {
+        return m_nodes[index].m_key;
+    }
+
+    //! Retrieves the value by index
+    /*!
+    */
+    inline T * get_value_by_index(GUINT index)
+    {
+        return &m_nodes[index].m_data;
+    }
+
+    inline const T& operator[](GUINT index) const
+    {
+        return m_nodes[index].m_data;
+    }
+
+    inline T& operator[](GUINT index)
+    {
+        return m_nodes[index].m_data;
+    }
+
+    //! Finds the index of the element with the key
+    /*!
+    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
+    If so, the element has been inserted at the last position of the array.
+    */
+    inline GUINT find(GUINT hashkey)
+    {
+        if(m_hash_table)
+        {
+            GUINT cell_index = _find_cell(hashkey);
+            if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH;
+            return m_hash_table[cell_index];
+        }
+		GUINT last_index = m_nodes.size();
+        if(last_index<2)
+        {
+			if(last_index==0) return GIM_INVALID_HASH;
+            if(m_nodes[0].m_key == hashkey) return 0;
+            return GIM_INVALID_HASH;
+        }
+        else if(m_sorted)
+        {
+            //Binary search
+            GUINT result_ind = 0;
+			last_index--;
+            _node_type *  ptr =  m_nodes.pointer();
+
+            bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());
+
+
+            if(found) return result_ind;
+        }
+        return GIM_INVALID_HASH;
+    }
+
+    //! Retrieves the value associated with the index
+    /*!
+    \return the found element, or null
+    */
+    inline T * get_value(GUINT hashkey)
+    {
+        GUINT index = find(hashkey);
+        if(index == GIM_INVALID_HASH) return NULL;
+        return &m_nodes[index].m_data;
+    }
+
+
+    /*!
+    */
+    inline bool erase_by_index(GUINT index)
+    {
+        if(index > m_nodes.size()) return false;
+
+        if(m_hash_table == NULL)
+        {
+            if(is_sorted())
+            {
+                return this->_erase_sorted(index);
+            }
+            else
+            {
+                return this->_erase_unsorted(index);
+            }
+        }
+        else
+        {
+            return this->_erase_by_index_hash_table(index);
+        }
+        return false;
+    }
+
+
+
+    inline bool erase_by_index_unsorted(GUINT index)
+    {
+        if(index > m_nodes.size()) return false;
+
+        if(m_hash_table == NULL)
+        {
+            return this->_erase_unsorted(index);
+        }
+        else
+        {
+            return this->_erase_by_index_hash_table(index);
+        }
+        return false;
+    }
+
+
+
+    /*!
+
+    */
+    inline bool erase_by_key(GUINT hashkey)
+    {
+        if(size()==0) return false;
+
+        if(m_hash_table)
+        {
+            return this->_erase_hash_table(hashkey);
+        }
+        //Binary search
+
+        if(is_sorted()==false) return false;
+
+        GUINT result_ind = find(hashkey);
+        if(result_ind!= GIM_INVALID_HASH)
+        {
+            return this->_erase_sorted(result_ind);
+        }
+        return false;
+    }
+
+    void clear()
+    {
+        m_nodes.clear();
+
+        if(m_hash_table==NULL) return;
+        GUINT datasize = m_table_size*m_node_size;
+        //Initialize the hashkeys.
+        GUINT i;
+        for(i=0;i<datasize;i++)
+        {
+            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
+        }
+		m_sorted = false;
+    }
+
+    //! Insert an element into the hash
+    /*!
+    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+    of the existing element.
+    */
+    inline GUINT insert(GUINT hashkey, const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table(hashkey,element);
+        }
+        if(this->is_sorted())
+        {
+            return this->_insert_sorted(hashkey,element);
+        }
+        return this->_insert_unsorted(hashkey,element);
+    }
+
+    //! Insert an element into the hash, and could overrite an existing object with the same hash.
+    /*!
+    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
+    of the replaced element.
+    */
+    inline GUINT insert_override(GUINT hashkey, const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table_replace(hashkey,element);
+        }
+        if(this->is_sorted())
+        {
+            return this->_insert_sorted_replace(hashkey,element);
+        }
+        this->_insert_unsorted(hashkey,element);
+        return m_nodes.size();
+    }
+
+
+
+    //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
+    /*!
+    */
+    inline GUINT insert_unsorted(GUINT hashkey,const T & element)
+    {
+        if(m_hash_table)
+        {
+            return this->_insert_hash_table(hashkey,element);
+        }
+        return this->_insert_unsorted(hashkey,element);
+    }
+
+
+};
+
+//! @}
+
+
+
+#endif // GIM_CONTAINERS_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_linear_math.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_linear_math.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_linear_math.h	(revision 1963)
@@ -0,0 +1,1584 @@
+#ifndef GIM_LINEAR_H_INCLUDED
+#define GIM_LINEAR_H_INCLUDED
+
+/*! \file gim_linear_math.h
+*\author Francisco Len Nßjera
+Type Independant Vector and matrix operations.
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+#include "gim_geom_types.h"
+
+
+
+/*! \defgroup VECTOR_OPERATIONS
+T
+Operations for vectors : vec2f,vec3f and vec4f
+*/
+//! @{
+
+//! Zero out a 2D vector
+#define VEC_ZERO_2(a)				\
+{						\
+   (a)[0] = (a)[1] = 0.0f;			\
+}\
+
+
+//! Zero out a 3D vector
+#define VEC_ZERO(a)				\
+{						\
+   (a)[0] = (a)[1] = (a)[2] = 0.0f;		\
+}\
+
+
+/// Zero out a 4D vector
+#define VEC_ZERO_4(a)				\
+{						\
+   (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;	\
+}\
+
+
+/// Vector copy
+#define VEC_COPY_2(b,a)				\
+{						\
+   (b)[0] = (a)[0];				\
+   (b)[1] = (a)[1];				\
+}\
+
+
+/// Copy 3D vector
+#define VEC_COPY(b,a)				\
+{						\
+   (b)[0] = (a)[0];				\
+   (b)[1] = (a)[1];				\
+   (b)[2] = (a)[2];				\
+}\
+
+
+/// Copy 4D vector
+#define VEC_COPY_4(b,a)				\
+{						\
+   (b)[0] = (a)[0];				\
+   (b)[1] = (a)[1];				\
+   (b)[2] = (a)[2];				\
+   (b)[3] = (a)[3];				\
+}\
+
+/// VECTOR SWAP
+#define VEC_SWAP(b,a)				\
+{  \
+    GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
+    GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
+    GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
+}\
+
+/// Vector difference
+#define VEC_DIFF_2(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] - (v1)[0];		\
+   (v21)[1] = (v2)[1] - (v1)[1];		\
+}\
+
+
+/// Vector difference
+#define VEC_DIFF(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] - (v1)[0];		\
+   (v21)[1] = (v2)[1] - (v1)[1];		\
+   (v21)[2] = (v2)[2] - (v1)[2];		\
+}\
+
+
+/// Vector difference
+#define VEC_DIFF_4(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] - (v1)[0];		\
+   (v21)[1] = (v2)[1] - (v1)[1];		\
+   (v21)[2] = (v2)[2] - (v1)[2];		\
+   (v21)[3] = (v2)[3] - (v1)[3];		\
+}\
+
+
+/// Vector sum
+#define VEC_SUM_2(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] + (v1)[0];		\
+   (v21)[1] = (v2)[1] + (v1)[1];		\
+}\
+
+
+/// Vector sum
+#define VEC_SUM(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] + (v1)[0];		\
+   (v21)[1] = (v2)[1] + (v1)[1];		\
+   (v21)[2] = (v2)[2] + (v1)[2];		\
+}\
+
+
+/// Vector sum
+#define VEC_SUM_4(v21,v2,v1)			\
+{						\
+   (v21)[0] = (v2)[0] + (v1)[0];		\
+   (v21)[1] = (v2)[1] + (v1)[1];		\
+   (v21)[2] = (v2)[2] + (v1)[2];		\
+   (v21)[3] = (v2)[3] + (v1)[3];		\
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE_2(c,a,b)			\
+{						\
+   (c)[0] = (a)*(b)[0];				\
+   (c)[1] = (a)*(b)[1];				\
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE(c,a,b)			\
+{						\
+   (c)[0] = (a)*(b)[0];				\
+   (c)[1] = (a)*(b)[1];				\
+   (c)[2] = (a)*(b)[2];				\
+}\
+
+
+/// scalar times vector
+#define VEC_SCALE_4(c,a,b)			\
+{						\
+   (c)[0] = (a)*(b)[0];				\
+   (c)[1] = (a)*(b)[1];				\
+   (c)[2] = (a)*(b)[2];				\
+   (c)[3] = (a)*(b)[3];				\
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM_2(c,a,b)			\
+{						\
+   (c)[0] += (a)*(b)[0];			\
+   (c)[1] += (a)*(b)[1];			\
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM(c,a,b)			\
+{						\
+   (c)[0] += (a)*(b)[0];			\
+   (c)[1] += (a)*(b)[1];			\
+   (c)[2] += (a)*(b)[2];			\
+}\
+
+
+/// accumulate scaled vector
+#define VEC_ACCUM_4(c,a,b)			\
+{						\
+   (c)[0] += (a)*(b)[0];			\
+   (c)[1] += (a)*(b)[1];			\
+   (c)[2] += (a)*(b)[2];			\
+   (c)[3] += (a)*(b)[3];			\
+}\
+
+
+/// Vector dot product
+#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])
+
+
+/// Vector dot product
+#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
+
+/// Vector dot product
+#define VEC_DOT_4(a,b)	((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
+
+/// vector impact parameter (squared)
+#define VEC_IMPACT_SQ(bsq,direction,position) {\
+   GREAL _llel_ = VEC_DOT(direction, position);\
+   bsq = VEC_DOT(position, position) - _llel_*_llel_;\
+}\
+
+
+/// vector impact parameter
+#define VEC_IMPACT(bsq,direction,position)	{\
+   VEC_IMPACT_SQ(bsq,direction,position);		\
+   GIM_SQRT(bsq,bsq);					\
+}\
+
+/// Vector length
+#define VEC_LENGTH_2(a,l)\
+{\
+    GREAL _pp = VEC_DOT_2(a,a);\
+    GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_LENGTH(a,l)\
+{\
+    GREAL _pp = VEC_DOT(a,a);\
+    GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_LENGTH_4(a,l)\
+{\
+    GREAL _pp = VEC_DOT_4(a,a);\
+    GIM_SQRT(_pp,l);\
+}\
+
+/// Vector inv length
+#define VEC_INV_LENGTH_2(a,l)\
+{\
+    GREAL _pp = VEC_DOT_2(a,a);\
+    GIM_INV_SQRT(_pp,l);\
+}\
+
+
+/// Vector inv length
+#define VEC_INV_LENGTH(a,l)\
+{\
+    GREAL _pp = VEC_DOT(a,a);\
+    GIM_INV_SQRT(_pp,l);\
+}\
+
+
+/// Vector inv length
+#define VEC_INV_LENGTH_4(a,l)\
+{\
+    GREAL _pp = VEC_DOT_4(a,a);\
+    GIM_INV_SQRT(_pp,l);\
+}\
+
+
+
+/// distance between two points
+#define VEC_DISTANCE(_len,_va,_vb) {\
+    vec3f _tmp_;				\
+    VEC_DIFF(_tmp_, _vb, _va);			\
+    VEC_LENGTH(_tmp_,_len);			\
+}\
+
+
+/// Vector length
+#define VEC_CONJUGATE_LENGTH(a,l)\
+{\
+    GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
+    GIM_SQRT(_pp,l);\
+}\
+
+
+/// Vector length
+#define VEC_NORMALIZE(a) {	\
+    GREAL len;\
+    VEC_INV_LENGTH(a,len); \
+    if(len<G_REAL_INFINITY)\
+    {\
+        a[0] *= len;				\
+        a[1] *= len;				\
+        a[2] *= len;				\
+    }						\
+}\
+
+/// Set Vector size
+#define VEC_RENORMALIZE(a,newlen) {	\
+    GREAL len;\
+    VEC_INV_LENGTH(a,len); \
+    if(len<G_REAL_INFINITY)\
+    {\
+        len *= newlen;\
+        a[0] *= len;				\
+        a[1] *= len;				\
+        a[2] *= len;				\
+    }						\
+}\
+
+/// Vector cross
+#define VEC_CROSS(c,a,b)		\
+{						\
+   c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];	\
+   c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];	\
+   c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];	\
+}\
+
+
+/*! Vector perp -- assumes that n is of unit length
+ * accepts vector v, subtracts out any component parallel to n */
+#define VEC_PERPENDICULAR(vp,v,n)			\
+{						\
+   GREAL dot = VEC_DOT(v, n);			\
+   vp[0] = (v)[0] - dot*(n)[0];		\
+   vp[1] = (v)[1] - dot*(n)[1];		\
+   vp[2] = (v)[2] - dot*(n)[2];		\
+}\
+
+
+/*! Vector parallel -- assumes that n is of unit length */
+#define VEC_PARALLEL(vp,v,n)			\
+{						\
+   GREAL dot = VEC_DOT(v, n);			\
+   vp[0] = (dot) * (n)[0];			\
+   vp[1] = (dot) * (n)[1];			\
+   vp[2] = (dot) * (n)[2];			\
+}\
+
+/*! Same as Vector parallel --  n can have any length
+ * accepts vector v, subtracts out any component perpendicular to n */
+#define VEC_PROJECT(vp,v,n)			\
+{ \
+	GREAL scalar = VEC_DOT(v, n);			\
+	scalar/= VEC_DOT(n, n); \
+	vp[0] = (scalar) * (n)[0];			\
+    vp[1] = (scalar) * (n)[1];			\
+    vp[2] = (scalar) * (n)[2];			\
+}\
+
+
+/*! accepts vector v*/
+#define VEC_UNPROJECT(vp,v,n)			\
+{ \
+	GREAL scalar = VEC_DOT(v, n);			\
+	scalar = VEC_DOT(n, n)/scalar; \
+	vp[0] = (scalar) * (n)[0];			\
+    vp[1] = (scalar) * (n)[1];			\
+    vp[2] = (scalar) * (n)[2];			\
+}\
+
+
+/*! Vector reflection -- assumes n is of unit length
+ Takes vector v, reflects it against reflector n, and returns vr */
+#define VEC_REFLECT(vr,v,n)			\
+{						\
+   GREAL dot = VEC_DOT(v, n);			\
+   vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];	\
+   vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];	\
+   vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];	\
+}\
+
+
+/*! Vector blending
+Takes two vectors a, b, blends them together with two scalars */
+#define VEC_BLEND_AB(vr,sa,a,sb,b)			\
+{						\
+   vr[0] = (sa) * (a)[0] + (sb) * (b)[0];	\
+   vr[1] = (sa) * (a)[1] + (sb) * (b)[1];	\
+   vr[2] = (sa) * (a)[2] + (sb) * (b)[2];	\
+}\
+
+/*! Vector blending
+Takes two vectors a, b, blends them together with s <=1 */
+#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)
+
+#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
+
+//! Finds the bigger cartesian coordinate from a vector
+#define VEC_MAYOR_COORD(vec, maxc)\
+{\
+	GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
+    maxc =  A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
+}\
+
+//! Finds the 2 smallest cartesian coordinates from a vector
+#define VEC_MINOR_AXES(vec, i0, i1)\
+{\
+	VEC_MAYOR_COORD(vec,i0);\
+	i0 = (i0+1)%3;\
+	i1 = (i0+1)%3;\
+}\
+
+
+
+
+#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])
+
+#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2]))
+
+
+/// Vector cross
+#define X_AXIS_CROSS_VEC(dst,src)\
+{					   \
+	dst[0] = 0.0f;     \
+	dst[1] = -src[2];  \
+	dst[2] = src[1];  \
+}\
+
+#define Y_AXIS_CROSS_VEC(dst,src)\
+{					   \
+	dst[0] = src[2];     \
+	dst[1] = 0.0f;  \
+	dst[2] = -src[0];  \
+}\
+
+#define Z_AXIS_CROSS_VEC(dst,src)\
+{					   \
+	dst[0] = -src[1];     \
+	dst[1] = src[0];  \
+	dst[2] = 0.0f;  \
+}\
+
+
+
+//! @}
+
+
+/*! \defgroup MATRIX_OPERATIONS
+Operations for matrices : mat2f, mat3f and mat4f
+*/
+//! @{
+
+/// initialize matrix
+#define IDENTIFY_MATRIX_3X3(m)			\
+{						\
+   m[0][0] = 1.0;				\
+   m[0][1] = 0.0;				\
+   m[0][2] = 0.0;				\
+						\
+   m[1][0] = 0.0;				\
+   m[1][1] = 1.0;				\
+   m[1][2] = 0.0;				\
+						\
+   m[2][0] = 0.0;				\
+   m[2][1] = 0.0;				\
+   m[2][2] = 1.0;				\
+}\
+
+/*! initialize matrix */
+#define IDENTIFY_MATRIX_4X4(m)			\
+{						\
+   m[0][0] = 1.0;				\
+   m[0][1] = 0.0;				\
+   m[0][2] = 0.0;				\
+   m[0][3] = 0.0;				\
+						\
+   m[1][0] = 0.0;				\
+   m[1][1] = 1.0;				\
+   m[1][2] = 0.0;				\
+   m[1][3] = 0.0;				\
+						\
+   m[2][0] = 0.0;				\
+   m[2][1] = 0.0;				\
+   m[2][2] = 1.0;				\
+   m[2][3] = 0.0;				\
+						\
+   m[3][0] = 0.0;				\
+   m[3][1] = 0.0;				\
+   m[3][2] = 0.0;				\
+   m[3][3] = 1.0;				\
+}\
+
+/*! initialize matrix */
+#define ZERO_MATRIX_4X4(m)			\
+{						\
+   m[0][0] = 0.0;				\
+   m[0][1] = 0.0;				\
+   m[0][2] = 0.0;				\
+   m[0][3] = 0.0;				\
+						\
+   m[1][0] = 0.0;				\
+   m[1][1] = 0.0;				\
+   m[1][2] = 0.0;				\
+   m[1][3] = 0.0;				\
+						\
+   m[2][0] = 0.0;				\
+   m[2][1] = 0.0;				\
+   m[2][2] = 0.0;				\
+   m[2][3] = 0.0;				\
+						\
+   m[3][0] = 0.0;				\
+   m[3][1] = 0.0;				\
+   m[3][2] = 0.0;				\
+   m[3][3] = 0.0;				\
+}\
+
+/*! matrix rotation  X */
+#define ROTX_CS(m,cosine,sine)		\
+{					\
+   /* rotation about the x-axis */	\
+					\
+   m[0][0] = 1.0;			\
+   m[0][1] = 0.0;			\
+   m[0][2] = 0.0;			\
+   m[0][3] = 0.0;			\
+					\
+   m[1][0] = 0.0;			\
+   m[1][1] = (cosine);			\
+   m[1][2] = (sine);			\
+   m[1][3] = 0.0;			\
+					\
+   m[2][0] = 0.0;			\
+   m[2][1] = -(sine);			\
+   m[2][2] = (cosine);			\
+   m[2][3] = 0.0;			\
+					\
+   m[3][0] = 0.0;			\
+   m[3][1] = 0.0;			\
+   m[3][2] = 0.0;			\
+   m[3][3] = 1.0;			\
+}\
+
+/*! matrix rotation  Y */
+#define ROTY_CS(m,cosine,sine)		\
+{					\
+   /* rotation about the y-axis */	\
+					\
+   m[0][0] = (cosine);			\
+   m[0][1] = 0.0;			\
+   m[0][2] = -(sine);			\
+   m[0][3] = 0.0;			\
+					\
+   m[1][0] = 0.0;			\
+   m[1][1] = 1.0;			\
+   m[1][2] = 0.0;			\
+   m[1][3] = 0.0;			\
+					\
+   m[2][0] = (sine);			\
+   m[2][1] = 0.0;			\
+   m[2][2] = (cosine);			\
+   m[2][3] = 0.0;			\
+					\
+   m[3][0] = 0.0;			\
+   m[3][1] = 0.0;			\
+   m[3][2] = 0.0;			\
+   m[3][3] = 1.0;			\
+}\
+
+/*! matrix rotation  Z */
+#define ROTZ_CS(m,cosine,sine)		\
+{					\
+   /* rotation about the z-axis */	\
+					\
+   m[0][0] = (cosine);			\
+   m[0][1] = (sine);			\
+   m[0][2] = 0.0;			\
+   m[0][3] = 0.0;			\
+					\
+   m[1][0] = -(sine);			\
+   m[1][1] = (cosine);			\
+   m[1][2] = 0.0;			\
+   m[1][3] = 0.0;			\
+					\
+   m[2][0] = 0.0;			\
+   m[2][1] = 0.0;			\
+   m[2][2] = 1.0;			\
+   m[2][3] = 0.0;			\
+					\
+   m[3][0] = 0.0;			\
+   m[3][1] = 0.0;			\
+   m[3][2] = 0.0;			\
+   m[3][3] = 1.0;			\
+}\
+
+/*! matrix copy */
+#define COPY_MATRIX_2X2(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[0][1];		\
+				\
+   b[1][0] = a[1][0];		\
+   b[1][1] = a[1][1];		\
+				\
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_2X3(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[0][1];		\
+   b[0][2] = a[0][2];		\
+				\
+   b[1][0] = a[1][0];		\
+   b[1][1] = a[1][1];		\
+   b[1][2] = a[1][2];		\
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_3X3(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[0][1];		\
+   b[0][2] = a[0][2];		\
+				\
+   b[1][0] = a[1][0];		\
+   b[1][1] = a[1][1];		\
+   b[1][2] = a[1][2];		\
+				\
+   b[2][0] = a[2][0];		\
+   b[2][1] = a[2][1];		\
+   b[2][2] = a[2][2];		\
+}\
+
+
+/*! matrix copy */
+#define COPY_MATRIX_4X4(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[0][1];		\
+   b[0][2] = a[0][2];		\
+   b[0][3] = a[0][3];		\
+				\
+   b[1][0] = a[1][0];		\
+   b[1][1] = a[1][1];		\
+   b[1][2] = a[1][2];		\
+   b[1][3] = a[1][3];		\
+				\
+   b[2][0] = a[2][0];		\
+   b[2][1] = a[2][1];		\
+   b[2][2] = a[2][2];		\
+   b[2][3] = a[2][3];		\
+				\
+   b[3][0] = a[3][0];		\
+   b[3][1] = a[3][1];		\
+   b[3][2] = a[3][2];		\
+   b[3][3] = a[3][3];		\
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_2X2(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[1][0];		\
+				\
+   b[1][0] = a[0][1];		\
+   b[1][1] = a[1][1];		\
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_3X3(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[1][0];		\
+   b[0][2] = a[2][0];		\
+				\
+   b[1][0] = a[0][1];		\
+   b[1][1] = a[1][1];		\
+   b[1][2] = a[2][1];		\
+				\
+   b[2][0] = a[0][2];		\
+   b[2][1] = a[1][2];		\
+   b[2][2] = a[2][2];		\
+}\
+
+
+/*! matrix transpose */
+#define TRANSPOSE_MATRIX_4X4(b,a)	\
+{				\
+   b[0][0] = a[0][0];		\
+   b[0][1] = a[1][0];		\
+   b[0][2] = a[2][0];		\
+   b[0][3] = a[3][0];		\
+				\
+   b[1][0] = a[0][1];		\
+   b[1][1] = a[1][1];		\
+   b[1][2] = a[2][1];		\
+   b[1][3] = a[3][1];		\
+				\
+   b[2][0] = a[0][2];		\
+   b[2][1] = a[1][2];		\
+   b[2][2] = a[2][2];		\
+   b[2][3] = a[3][2];		\
+				\
+   b[3][0] = a[0][3];		\
+   b[3][1] = a[1][3];		\
+   b[3][2] = a[2][3];		\
+   b[3][3] = a[3][3];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_2X2(b,s,a)		\
+{					\
+   b[0][0] = (s) * a[0][0];		\
+   b[0][1] = (s) * a[0][1];		\
+					\
+   b[1][0] = (s) * a[1][0];		\
+   b[1][1] = (s) * a[1][1];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_3X3(b,s,a)		\
+{					\
+   b[0][0] = (s) * a[0][0];		\
+   b[0][1] = (s) * a[0][1];		\
+   b[0][2] = (s) * a[0][2];		\
+					\
+   b[1][0] = (s) * a[1][0];		\
+   b[1][1] = (s) * a[1][1];		\
+   b[1][2] = (s) * a[1][2];		\
+					\
+   b[2][0] = (s) * a[2][0];		\
+   b[2][1] = (s) * a[2][1];		\
+   b[2][2] = (s) * a[2][2];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_MATRIX_4X4(b,s,a)		\
+{					\
+   b[0][0] = (s) * a[0][0];		\
+   b[0][1] = (s) * a[0][1];		\
+   b[0][2] = (s) * a[0][2];		\
+   b[0][3] = (s) * a[0][3];		\
+					\
+   b[1][0] = (s) * a[1][0];		\
+   b[1][1] = (s) * a[1][1];		\
+   b[1][2] = (s) * a[1][2];		\
+   b[1][3] = (s) * a[1][3];		\
+					\
+   b[2][0] = (s) * a[2][0];		\
+   b[2][1] = (s) * a[2][1];		\
+   b[2][2] = (s) * a[2][2];		\
+   b[2][3] = (s) * a[2][3];		\
+					\
+   b[3][0] = s * a[3][0];		\
+   b[3][1] = s * a[3][1];		\
+   b[3][2] = s * a[3][2];		\
+   b[3][3] = s * a[3][3];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_VEC_MATRIX_2X2(b,svec,a)		\
+{					\
+   b[0][0] = svec[0] * a[0][0];		\
+   b[1][0] = svec[0] * a[1][0];		\
+					\
+   b[0][1] = svec[1] * a[0][1];		\
+   b[1][1] = svec[1] * a[1][1];		\
+}\
+
+
+/*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
+#define SCALE_VEC_MATRIX_3X3(b,svec,a)		\
+{					\
+   b[0][0] = svec[0] * a[0][0];		\
+   b[1][0] = svec[0] * a[1][0];		\
+   b[2][0] = svec[0] * a[2][0];		\
+					\
+   b[0][1] = svec[1] * a[0][1];		\
+   b[1][1] = svec[1] * a[1][1];		\
+   b[2][1] = svec[1] * a[2][1];		\
+					\
+   b[0][2] = svec[2] * a[0][2];		\
+   b[1][2] = svec[2] * a[1][2];		\
+   b[2][2] = svec[2] * a[2][2];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define SCALE_VEC_MATRIX_4X4(b,svec,a)		\
+{					\
+   b[0][0] = svec[0] * a[0][0];		\
+   b[1][0] = svec[0] * a[1][0];		\
+   b[2][0] = svec[0] * a[2][0];		\
+   b[3][0] = svec[0] * a[3][0];		\
+					\
+   b[0][1] = svec[1] * a[0][1];		\
+   b[1][1] = svec[1] * a[1][1];		\
+   b[2][1] = svec[1] * a[2][1];		\
+   b[3][1] = svec[1] * a[3][1];		\
+					\
+   b[0][2] = svec[2] * a[0][2];		\
+   b[1][2] = svec[2] * a[1][2];		\
+   b[2][2] = svec[2] * a[2][2];		\
+   b[3][2] = svec[2] * a[3][2];		\
+   \
+   b[0][3] = svec[3] * a[0][3];		\
+   b[1][3] = svec[3] * a[1][3];		\
+   b[2][3] = svec[3] * a[2][3];		\
+   b[3][3] = svec[3] * a[3][3];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_2X2(b,s,a)		\
+{					\
+   b[0][0] += (s) * a[0][0];		\
+   b[0][1] += (s) * a[0][1];		\
+					\
+   b[1][0] += (s) * a[1][0];		\
+   b[1][1] += (s) * a[1][1];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_3X3(b,s,a)		\
+{					\
+   b[0][0] += (s) * a[0][0];		\
+   b[0][1] += (s) * a[0][1];		\
+   b[0][2] += (s) * a[0][2];		\
+					\
+   b[1][0] += (s) * a[1][0];		\
+   b[1][1] += (s) * a[1][1];		\
+   b[1][2] += (s) * a[1][2];		\
+					\
+   b[2][0] += (s) * a[2][0];		\
+   b[2][1] += (s) * a[2][1];		\
+   b[2][2] += (s) * a[2][2];		\
+}\
+
+
+/*! multiply matrix by scalar */
+#define ACCUM_SCALE_MATRIX_4X4(b,s,a)		\
+{					\
+   b[0][0] += (s) * a[0][0];		\
+   b[0][1] += (s) * a[0][1];		\
+   b[0][2] += (s) * a[0][2];		\
+   b[0][3] += (s) * a[0][3];		\
+					\
+   b[1][0] += (s) * a[1][0];		\
+   b[1][1] += (s) * a[1][1];		\
+   b[1][2] += (s) * a[1][2];		\
+   b[1][3] += (s) * a[1][3];		\
+					\
+   b[2][0] += (s) * a[2][0];		\
+   b[2][1] += (s) * a[2][1];		\
+   b[2][2] += (s) * a[2][2];		\
+   b[2][3] += (s) * a[2][3];		\
+					\
+   b[3][0] += (s) * a[3][0];		\
+   b[3][1] += (s) * a[3][1];		\
+   b[3][2] += (s) * a[3][2];		\
+   b[3][3] += (s) * a[3][3];		\
+}\
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_2X2(c,a,b)		\
+{						\
+   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];	\
+   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];	\
+						\
+   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];	\
+   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];	\
+						\
+}\
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_3X3(c,a,b)				\
+{								\
+   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];	\
+   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];	\
+   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];	\
+								\
+   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];	\
+   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];	\
+   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];	\
+								\
+   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];	\
+   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];	\
+   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];	\
+}\
+
+
+/*! matrix product */
+/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
+#define MATRIX_PRODUCT_4X4(c,a,b)		\
+{						\
+   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
+   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
+   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
+   c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
+						\
+   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
+   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
+   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
+   c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
+						\
+   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
+   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
+   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
+   c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
+						\
+   c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
+   c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
+   c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
+   c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
+}\
+
+
+/*! matrix times vector */
+#define MAT_DOT_VEC_2X2(p,m,v)					\
+{								\
+   p[0] = m[0][0]*v[0] + m[0][1]*v[1];				\
+   p[1] = m[1][0]*v[0] + m[1][1]*v[1];				\
+}\
+
+
+/*! matrix times vector */
+#define MAT_DOT_VEC_3X3(p,m,v)					\
+{								\
+   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];		\
+   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];		\
+   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];		\
+}\
+
+
+/*! matrix times vector
+v is a vec4f
+*/
+#define MAT_DOT_VEC_4X4(p,m,v)					\
+{								\
+   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];	\
+   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];	\
+   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];	\
+   p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];	\
+}\
+
+/*! matrix times vector
+v is a vec3f
+and m is a mat4f<br>
+Last column is added as the position
+*/
+#define MAT_DOT_VEC_3X4(p,m,v)					\
+{								\
+   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3];	\
+   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3];	\
+   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3];	\
+}\
+
+
+/*! vector transpose times matrix */
+/*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
+#define VEC_DOT_MAT_3X3(p,v,m)					\
+{								\
+   p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];		\
+   p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];		\
+   p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];		\
+}\
+
+
+/*! affine matrix times vector */
+/** The matrix is assumed to be an affine matrix, with last two
+ * entries representing a translation */
+#define MAT_DOT_VEC_2X3(p,m,v)					\
+{								\
+   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];		\
+   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];		\
+}\
+
+//! Transform a plane
+#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
+{								\
+   pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1]  + m[0][2]*plane[2];\
+   pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1]  + m[1][2]*plane[2];\
+   pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1]  + m[2][2]*plane[2];\
+   pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1]  + m[2][3]*pout[2] + plane[3];\
+}\
+
+
+
+/** inverse transpose of matrix times vector
+ *
+ * This macro computes inverse transpose of matrix m,
+ * and multiplies vector v into it, to yeild vector p
+ *
+ * DANGER !!! Do Not use this on normal vectors!!!
+ * It will leave normals the wrong length !!!
+ * See macro below for use on normals.
+ */
+#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)			\
+{								\
+   GREAL det;						\
+								\
+   det = m[0][0]*m[1][1] - m[0][1]*m[1][0];			\
+   p[0] = m[1][1]*v[0] - m[1][0]*v[1];				\
+   p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
+								\
+   /* if matrix not singular, and not orthonormal, then renormalize */ \
+   if ((det!=1.0f) && (det != 0.0f)) {				\
+      det = 1.0f / det;						\
+      p[0] *= det;						\
+      p[1] *= det;						\
+   }								\
+}\
+
+
+/** transform normal vector by inverse transpose of matrix
+ * and then renormalize the vector
+ *
+ * This macro computes inverse transpose of matrix m,
+ * and multiplies vector v into it, to yeild vector p
+ * Vector p is then normalized.
+ */
+#define NORM_XFORM_2X2(p,m,v)					\
+{								\
+   GREAL len;							\
+								\
+   /* do nothing if off-diagonals are zero and diagonals are 	\
+    * equal */							\
+   if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
+      p[0] = m[1][1]*v[0] - m[1][0]*v[1];			\
+      p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
+								\
+      len = p[0]*p[0] + p[1]*p[1];				\
+      GIM_INV_SQRT(len,len);					\
+      p[0] *= len;						\
+      p[1] *= len;						\
+   } else {							\
+      VEC_COPY_2 (p, v);					\
+   }								\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_2X2(m,v,t)				\
+{								\
+   m[0][0] = v[0] * t[0];					\
+   m[0][1] = v[0] * t[1];					\
+								\
+   m[1][0] = v[1] * t[0];					\
+   m[1][1] = v[1] * t[1];					\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_3X3(m,v,t)				\
+{								\
+   m[0][0] = v[0] * t[0];					\
+   m[0][1] = v[0] * t[1];					\
+   m[0][2] = v[0] * t[2];					\
+								\
+   m[1][0] = v[1] * t[0];					\
+   m[1][1] = v[1] * t[1];					\
+   m[1][2] = v[1] * t[2];					\
+								\
+   m[2][0] = v[2] * t[0];					\
+   m[2][1] = v[2] * t[1];					\
+   m[2][2] = v[2] * t[2];					\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define OUTER_PRODUCT_4X4(m,v,t)				\
+{								\
+   m[0][0] = v[0] * t[0];					\
+   m[0][1] = v[0] * t[1];					\
+   m[0][2] = v[0] * t[2];					\
+   m[0][3] = v[0] * t[3];					\
+								\
+   m[1][0] = v[1] * t[0];					\
+   m[1][1] = v[1] * t[1];					\
+   m[1][2] = v[1] * t[2];					\
+   m[1][3] = v[1] * t[3];					\
+								\
+   m[2][0] = v[2] * t[0];					\
+   m[2][1] = v[2] * t[1];					\
+   m[2][2] = v[2] * t[2];					\
+   m[2][3] = v[2] * t[3];					\
+								\
+   m[3][0] = v[3] * t[0];					\
+   m[3][1] = v[3] * t[1];					\
+   m[3][2] = v[3] * t[2];					\
+   m[3][3] = v[3] * t[3];					\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_2X2(m,v,t)				\
+{								\
+   m[0][0] += v[0] * t[0];					\
+   m[0][1] += v[0] * t[1];					\
+								\
+   m[1][0] += v[1] * t[0];					\
+   m[1][1] += v[1] * t[1];					\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_3X3(m,v,t)				\
+{								\
+   m[0][0] += v[0] * t[0];					\
+   m[0][1] += v[0] * t[1];					\
+   m[0][2] += v[0] * t[2];					\
+								\
+   m[1][0] += v[1] * t[0];					\
+   m[1][1] += v[1] * t[1];					\
+   m[1][2] += v[1] * t[2];					\
+								\
+   m[2][0] += v[2] * t[0];					\
+   m[2][1] += v[2] * t[1];					\
+   m[2][2] += v[2] * t[2];					\
+}\
+
+
+/** outer product of vector times vector transpose
+ *
+ * The outer product of vector v and vector transpose t yeilds
+ * dyadic matrix m.
+ */
+#define ACCUM_OUTER_PRODUCT_4X4(m,v,t)				\
+{								\
+   m[0][0] += v[0] * t[0];					\
+   m[0][1] += v[0] * t[1];					\
+   m[0][2] += v[0] * t[2];					\
+   m[0][3] += v[0] * t[3];					\
+								\
+   m[1][0] += v[1] * t[0];					\
+   m[1][1] += v[1] * t[1];					\
+   m[1][2] += v[1] * t[2];					\
+   m[1][3] += v[1] * t[3];					\
+								\
+   m[2][0] += v[2] * t[0];					\
+   m[2][1] += v[2] * t[1];					\
+   m[2][2] += v[2] * t[2];					\
+   m[2][3] += v[2] * t[3];					\
+								\
+   m[3][0] += v[3] * t[0];					\
+   m[3][1] += v[3] * t[1];					\
+   m[3][2] += v[3] * t[2];					\
+   m[3][3] += v[3] * t[3];					\
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_2X2(d,m)					\
+{								\
+   d = m[0][0] * m[1][1] - m[0][1] * m[1][0];			\
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_3X3(d,m)					\
+{								\
+   d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);		\
+   d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);	\
+   d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);	\
+}\
+
+
+/** i,j,th cofactor of a 4x4 matrix
+ *
+ */
+#define COFACTOR_4X4_IJ(fac,m,i,j) 				\
+{								\
+   GUINT __ii[4], __jj[4], __k;						\
+								\
+   for (__k=0; __k<i; __k++) __ii[__k] = __k;				\
+   for (__k=i; __k<3; __k++) __ii[__k] = __k+1;				\
+   for (__k=0; __k<j; __k++) __jj[__k] = __k;				\
+   for (__k=j; __k<3; __k++) __jj[__k] = __k+1;				\
+								\
+   (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] 	\
+                            - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
+   (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]	\
+                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
+   (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]	\
+                             - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
+								\
+   __k = i+j;							\
+   if ( __k != (__k/2)*2) {						\
+      (fac) = -(fac);						\
+   }								\
+}\
+
+
+/** determinant of matrix
+ *
+ * Computes determinant of matrix m, returning d
+ */
+#define DETERMINANT_4X4(d,m)					\
+{								\
+   GREAL cofac;						\
+   COFACTOR_4X4_IJ (cofac, m, 0, 0);				\
+   d = m[0][0] * cofac;						\
+   COFACTOR_4X4_IJ (cofac, m, 0, 1);				\
+   d += m[0][1] * cofac;					\
+   COFACTOR_4X4_IJ (cofac, m, 0, 2);				\
+   d += m[0][2] * cofac;					\
+   COFACTOR_4X4_IJ (cofac, m, 0, 3);				\
+   d += m[0][3] * cofac;					\
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_2X2(a,m)					\
+{								\
+   a[0][0] = (m)[1][1];						\
+   a[0][1] = - (m)[1][0];						\
+   a[1][0] = - (m)[0][1];						\
+   a[1][1] = (m)[0][0];						\
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_3X3(a,m)					\
+{								\
+   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
+   a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
+   a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
+   a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
+   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
+   a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
+   a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
+   a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
+   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
+}\
+
+
+/** cofactor of matrix
+ *
+ * Computes cofactor of matrix m, returning a
+ */
+#define COFACTOR_4X4(a,m)					\
+{								\
+   int i,j;							\
+								\
+   for (i=0; i<4; i++) {					\
+      for (j=0; j<4; j++) {					\
+         COFACTOR_4X4_IJ (a[i][j], m, i, j);			\
+      }								\
+   }								\
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_2X2(a,m)					\
+{								\
+   a[0][0] = (m)[1][1];						\
+   a[1][0] = - (m)[1][0];						\
+   a[0][1] = - (m)[0][1];						\
+   a[1][1] = (m)[0][0];						\
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_3X3(a,m)					\
+{								\
+   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
+   a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
+   a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
+   a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
+   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
+   a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
+   a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
+   a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
+   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
+}\
+
+
+/** adjoint of matrix
+ *
+ * Computes adjoint of matrix m, returning a
+ * (Note that adjoint is just the transpose of the cofactor matrix)
+ */
+#define ADJOINT_4X4(a,m)					\
+{								\
+   char _i_,_j_;							\
+								\
+   for (_i_=0; _i_<4; _i_++) {					\
+      for (_j_=0; _j_<4; _j_++) {					\
+         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
+      }								\
+   }								\
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_2X2(a,s,m)				\
+{								\
+   a[0][0] = (s) * m[1][1];					\
+   a[1][0] = - (s) * m[1][0];					\
+   a[0][1] = - (s) * m[0][1];					\
+   a[1][1] = (s) * m[0][0];					\
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_3X3(a,s,m)				\
+{								\
+   a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);	\
+   a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);	\
+   a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);	\
+								\
+   a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);	\
+   a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);	\
+   a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);	\
+								\
+   a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);	\
+   a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);	\
+   a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);	\
+}\
+
+
+/** compute adjoint of matrix and scale
+ *
+ * Computes adjoint of matrix m, scales it by s, returning a
+ */
+#define SCALE_ADJOINT_4X4(a,s,m)				\
+{								\
+   char _i_,_j_; \
+   for (_i_=0; _i_<4; _i_++) {					\
+      for (_j_=0; _j_<4; _j_++) {					\
+         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
+         a[_j_][_i_] *= s;						\
+      }								\
+   }								\
+}\
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_2X2(b,det,a)			\
+{						\
+   GREAL _tmp_;					\
+   DETERMINANT_2X2 (det, a);			\
+   _tmp_ = 1.0 / (det);				\
+   SCALE_ADJOINT_2X2 (b, _tmp_, a);		\
+}\
+
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_3X3(b,det,a)			\
+{						\
+   GREAL _tmp_;					\
+   DETERMINANT_3X3 (det, a);			\
+   _tmp_ = 1.0 / (det);				\
+   SCALE_ADJOINT_3X3 (b, _tmp_, a);		\
+}\
+
+
+/** inverse of matrix
+ *
+ * Compute inverse of matrix a, returning determinant m and
+ * inverse b
+ */
+#define INVERT_4X4(b,det,a)			\
+{						\
+   GREAL _tmp_;					\
+   DETERMINANT_4X4 (det, a);			\
+   _tmp_ = 1.0 / (det);				\
+   SCALE_ADJOINT_4X4 (b, _tmp_, a);		\
+}\
+
+//! Get the triple(3) row of a transform matrix
+#define MAT_GET_ROW(mat,vec3,rowindex)\
+{\
+    vec3[0] = mat[rowindex][0];\
+    vec3[1] = mat[rowindex][1];\
+    vec3[2] = mat[rowindex][2]; \
+}\
+
+//! Get the tuple(2) row of a transform matrix
+#define MAT_GET_ROW2(mat,vec2,rowindex)\
+{\
+    vec2[0] = mat[rowindex][0];\
+    vec2[1] = mat[rowindex][1];\
+}\
+
+
+//! Get the quad (4) row of a transform matrix
+#define MAT_GET_ROW4(mat,vec4,rowindex)\
+{\
+    vec4[0] = mat[rowindex][0];\
+    vec4[1] = mat[rowindex][1];\
+    vec4[2] = mat[rowindex][2];\
+    vec4[3] = mat[rowindex][3];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_COL(mat,vec3,colindex)\
+{\
+    vec3[0] = mat[0][colindex];\
+    vec3[1] = mat[1][colindex];\
+    vec3[2] = mat[2][colindex]; \
+}\
+
+//! Get the tuple(2) col of a transform matrix
+#define MAT_GET_COL2(mat,vec2,colindex)\
+{\
+    vec2[0] = mat[0][colindex];\
+    vec2[1] = mat[1][colindex];\
+}\
+
+
+//! Get the quad (4) col of a transform matrix
+#define MAT_GET_COL4(mat,vec4,colindex)\
+{\
+    vec4[0] = mat[0][colindex];\
+    vec4[1] = mat[1][colindex];\
+    vec4[2] = mat[2][colindex];\
+    vec4[3] = mat[3][colindex];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_X(mat,vec3)\
+{\
+    MAT_GET_COL(mat,vec3,0);\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_Y(mat,vec3)\
+{\
+    MAT_GET_COL(mat,vec3,1);\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_Z(mat,vec3)\
+{\
+    MAT_GET_COL(mat,vec3,2);\
+}\
+
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_X(mat,vec3)\
+{\
+    mat[0][0] = vec3[0];\
+    mat[1][0] = vec3[1];\
+    mat[2][0] = vec3[2];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_Y(mat,vec3)\
+{\
+    mat[0][1] = vec3[0];\
+    mat[1][1] = vec3[1];\
+    mat[2][1] = vec3[2];\
+}\
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_SET_Z(mat,vec3)\
+{\
+    mat[0][2] = vec3[0];\
+    mat[1][2] = vec3[1];\
+    mat[2][2] = vec3[2];\
+}\
+
+
+//! Get the triple(3) col of a transform matrix
+#define MAT_GET_TRANSLATION(mat,vec3)\
+{\
+    vec3[0] = mat[0][3];\
+    vec3[1] = mat[1][3];\
+    vec3[2] = mat[2][3]; \
+}\
+
+//! Set the triple(3) col of a transform matrix
+#define MAT_SET_TRANSLATION(mat,vec3)\
+{\
+    mat[0][3] = vec3[0];\
+    mat[1][3] = vec3[1];\
+    mat[2][3] = vec3[2]; \
+}\
+
+
+
+//! Returns the dot product between a vec3f and the row of a matrix
+#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])
+
+//! Returns the dot product between a vec2f and the row of a matrix
+#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])
+
+//! Returns the dot product between a vec4f and the row of a matrix
+#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3])
+
+
+//! Returns the dot product between a vec3f and the col of a matrix
+#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])
+
+//! Returns the dot product between a vec2f and the col of a matrix
+#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])
+
+//! Returns the dot product between a vec4f and the col of a matrix
+#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex])
+
+/*!Transpose matrix times vector
+v is a vec3f
+and m is a mat4f<br>
+*/
+#define INV_MAT_DOT_VEC_3X3(p,m,v)					\
+{								\
+   p[0] = MAT_DOT_COL(m,v,0); \
+   p[1] = MAT_DOT_COL(m,v,1);	\
+   p[2] = MAT_DOT_COL(m,v,2);	\
+}\
+
+
+//! @}
+
+#endif // GIM_VECTOR_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_math.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_math.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_math.h	(revision 1963)
@@ -0,0 +1,181 @@
+#ifndef GIM_MATH_H_INCLUDED
+#define GIM_MATH_H_INCLUDED
+/*! \file gim_math.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "LinearMath/btScalar.h"
+
+
+/*! \defgroup BASIC_TYPES
+Basic types and constants
+Conventions:
+Types starting with G
+Constants starting with G_
+*/
+//! @{
+
+#define GREAL btScalar
+#define GREAL2 double
+#define GINT int
+#define GUINT unsigned int
+#define GSHORT short
+#define GUSHORT unsigned short
+#define GINT64 long long
+#define GUINT64 unsigned long long
+
+//! @}
+
+/*! \defgroup BASIC_CONSTANTS
+Basic constants
+Conventions:
+Types starting with G
+Constants starting with G_
+*/
+//! @{
+
+#define G_PI 3.14159265358979f
+#define G_HALF_PI 1.5707963f
+//267948966
+#define G_TWO_PI 6.28318530f
+//71795864
+#define G_ROOT3 1.73205f
+#define G_ROOT2 1.41421f
+#define G_UINT_INFINITY 0xffffffff //!< A very very high value
+#define G_REAL_INFINITY FLT_MAX
+#define	G_SIGN_BITMASK			0x80000000
+#define G_EPSILON SIMD_EPSILON
+//! @}
+
+
+/*! \defgroup SCALAR_TYPES
+\brief
+Precision type constants
+*/
+//! @{
+enum GIM_SCALAR_TYPES
+{
+	G_STYPE_REAL =0,
+	G_STYPE_REAL2,
+	G_STYPE_SHORT,
+	G_STYPE_USHORT,
+	G_STYPE_INT,
+	G_STYPE_UINT,
+	G_STYPE_INT64,
+	G_STYPE_UINT64
+};
+//! @}
+
+/*! \defgroup MATH_FUNCTIONS
+mathematical functions
+*/
+//! @{
+
+#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
+#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)
+
+//! Integer representation of a floating-point value.
+#define GIM_IR(x)					((GUINT&)(x))
+
+//! Signed integer representation of a floating-point value.
+#define GIM_SIR(x)					((GINT&)(x))
+
+//! Absolute integer representation of a floating-point value
+#define GIM_AIR(x)					(GIM_IR(x)&0x7fffffff)
+
+//! Floating-point representation of an integer value.
+#define GIM_FR(x)					((GREAL&)(x))
+
+#define GIM_MAX(a,b) (a<b?b:a)
+#define GIM_MIN(a,b) (a>b?b:a)
+
+#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
+#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))
+
+#define GIM_IS_ZERO(value) (value < G_EPSILON &&  value > -G_EPSILON)
+
+#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)
+
+#define GIM_IS_POSISITVE(value) (value >= G_EPSILON)
+
+#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))
+
+///returns a clamped number
+#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
+
+#define GIM_GREATER(x, y)	fabsf(x) > (y)
+
+///Swap numbers
+#define GIM_SWAP_NUMBERS(a,b){ \
+    a = a+b; \
+    b = a-b; \
+    a = a-b; \
+}\
+
+#define GIM_INV_SQRT(va,isva)\
+{\
+    if(va<=0.0000001f)\
+    {\
+        isva = G_REAL_INFINITY;\
+    }\
+    else\
+    {\
+        GREAL _x = va * 0.5f;\
+        GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
+        isva = GIM_FR(_y);\
+        isva  = isva * ( 1.5f - ( _x * isva * isva ) );\
+    }\
+}\
+
+#define GIM_SQRT(va,sva)\
+{\
+    GIM_INV_SQRT(va,sva);\
+    sva = 1.0f/sva;\
+}\
+
+//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
+inline GREAL gim_inv_sqrt(GREAL f)
+{
+    GREAL r;
+    GIM_INV_SQRT(f,r);
+    return r;
+}
+
+inline GREAL gim_sqrt(GREAL f)
+{
+    GREAL r;
+    GIM_SQRT(f,r);
+    return r;
+}
+
+//! @}
+
+#endif // GIM_MATH_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.cpp	(revision 1963)
@@ -0,0 +1,135 @@
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_memory.h"
+#include "stdlib.h"
+
+#ifdef GIM_SIMD_MEMORY
+#include "LinearMath/btAlignedAllocator.h"
+#endif
+
+static gim_alloc_function *g_allocfn = 0;
+static gim_alloca_function *g_allocafn = 0;
+static gim_realloc_function *g_reallocfn = 0;
+static gim_free_function *g_freefn = 0;
+
+void gim_set_alloc_handler (gim_alloc_function *fn)
+{
+  g_allocfn = fn;
+}
+
+void gim_set_alloca_handler (gim_alloca_function *fn)
+{
+  g_allocafn = fn;
+}
+
+void gim_set_realloc_handler (gim_realloc_function *fn)
+{
+  g_reallocfn = fn;
+}
+
+void gim_set_free_handler (gim_free_function *fn)
+{
+  g_freefn = fn;
+}
+
+gim_alloc_function *gim_get_alloc_handler()
+{
+  return g_allocfn;
+}
+
+gim_alloca_function *gim_get_alloca_handler()
+{
+  return g_allocafn;
+}
+
+
+gim_realloc_function *gim_get_realloc_handler ()
+{
+  return g_reallocfn;
+}
+
+
+gim_free_function  *gim_get_free_handler ()
+{
+  return g_freefn;
+}
+
+
+void * gim_alloc(size_t size)
+{
+	void * ptr;
+	if (g_allocfn)
+	{
+		ptr = g_allocfn(size);
+	}
+	else
+	{
+#ifdef GIM_SIMD_MEMORY
+		ptr = btAlignedAlloc(size,16);
+#else
+		ptr = malloc(size);
+#endif
+	}
+  	return ptr;
+}
+
+void * gim_alloca(size_t size)
+{
+  if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
+}
+
+
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
+{
+ 	void * newptr = gim_alloc(newsize);
+    size_t copysize = oldsize<newsize?oldsize:newsize;
+    gim_simd_memcpy(newptr,ptr,copysize);
+    gim_free(ptr);
+    return newptr;
+}
+
+void gim_free(void *ptr)
+{
+	if (!ptr) return;
+	if (g_freefn)
+	{
+	   g_freefn(ptr);
+	}
+	else
+	{
+	#ifdef GIM_SIMD_MEMORY
+		btAlignedFree(ptr);
+	#else
+		free(ptr);
+	#endif
+	}
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_memory.h	(revision 1963)
@@ -0,0 +1,211 @@
+#ifndef GIM_MEMORY_H_INCLUDED
+#define GIM_MEMORY_H_INCLUDED
+/*! \file gim_memory.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+
+#include "gim_math.h"
+#include <memory.h>
+
+//#define PREFETCH 1
+//! \defgroup PREFETCH
+//! @{
+#ifdef PREFETCH
+#include <xmmintrin.h>	// for prefetch
+#define pfval	64
+#define pfval2	128
+//! Prefetch 64
+#define pf(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval), 0)
+//! Prefetch 128
+#define pf2(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval2), 0)
+#else
+//! Prefetch 64
+#define pf(_x,_i)
+//! Prefetch 128
+#define pf2(_x,_i)
+#endif
+//! @}
+
+/*! \defgroup ARRAY_UTILITIES
+\brief
+Functions for manip packed arrays of numbers
+*/
+//! @{
+#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	dest_array[_i_] = source_array[_i_];\
+    }\
+}\
+
+#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	copy_macro(dest_array[_i_],source_array[_i_]);\
+    }\
+}\
+
+
+#define GIM_ZERO_ARRAY(array,element_count)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	array[_i_] = 0;\
+    }\
+}\
+
+#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
+{\
+    for (GUINT _i_=0;_i_<element_count ;++_i_)\
+    {\
+    	array[_i_] = constant;\
+    }\
+}\
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_PROTOTYPES
+Function prototypes to allocate and free memory.
+*/
+//! @{
+typedef void * gim_alloc_function (size_t size);
+typedef void * gim_alloca_function (size_t size);//Allocs on the heap
+typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
+typedef void gim_free_function (void *ptr);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_HANDLERS
+\brief
+Memory Function Handlers
+ set new memory management functions. if fn is 0, the default handlers are
+  used. */
+//! @{
+void gim_set_alloc_handler (gim_alloc_function *fn);
+void gim_set_alloca_handler (gim_alloca_function *fn);
+void gim_set_realloc_handler (gim_realloc_function *fn);
+void gim_set_free_handler (gim_free_function *fn);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTION_GET_HANDLERS
+\brief
+get current memory management functions.
+*/
+//! @{
+gim_alloc_function *gim_get_alloc_handler (void);
+gim_alloca_function *gim_get_alloca_handler(void);
+gim_realloc_function *gim_get_realloc_handler (void);
+gim_free_function  *gim_get_free_handler (void);
+//! @}
+
+/*! \defgroup MEMORY_FUNCTIONS
+Standar Memory functions
+*/
+//! @{
+void * gim_alloc(size_t size);
+void * gim_alloca(size_t size);
+void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
+void gim_free(void *ptr);
+//! @}
+
+
+#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
+    #define GIM_SIMD_MEMORY 1
+#endif
+
+//! SIMD POINTER INTEGER
+#define SIMD_T GUINT64
+//! SIMD INTEGER SIZE
+#define SIMD_T_SIZE sizeof(SIMD_T)
+
+
+inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
+{
+#ifdef GIM_SIMD_MEMORY
+/*
+//'long long int' is incompatible with visual studio 6...
+    //copy words
+    SIMD_T * ui_src_ptr = (SIMD_T *)src;
+    SIMD_T * ui_dst_ptr = (SIMD_T *)dst;
+    while(copysize>=SIMD_T_SIZE)
+    {
+        *(ui_dst_ptr++) = *(ui_src_ptr++);
+        copysize-=SIMD_T_SIZE;
+    }
+    if(copysize==0) return;
+*/
+
+    char * c_src_ptr = (char *)src;
+    char * c_dst_ptr = (char *)dst;
+    while(copysize>0)
+    {
+        *(c_dst_ptr++) = *(c_src_ptr++);
+        copysize--;
+    }
+    return;
+#else
+    memcpy(dst,src,copysize);
+#endif
+}
+
+
+
+template<class T>
+inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
+{
+	T _e_tmp_ = _array[_i];
+	_array[_i] = _array[_j];
+	_array[_j] = _e_tmp_;
+}
+
+
+template<class T>
+inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
+{
+	char _e_tmp_[sizeof(T)];
+	gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
+	gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
+	gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
+}
+
+template <int SIZE>
+inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
+{
+	char _e_tmp_[SIZE];
+	_i*=SIZE;
+	_j*=SIZE;
+	gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
+	gim_simd_memcpy(_array+_i,_array+_j,SIZE);
+	gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
+}
+
+#endif // GIM_MEMORY_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_radixsort.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_radixsort.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_radixsort.h	(revision 1963)
@@ -0,0 +1,410 @@
+#ifndef GIM_RADIXSORT_H_INCLUDED
+#define GIM_RADIXSORT_H_INCLUDED
+/*! \file gim_radixsort.h
+\author Francisco Len Nßjera.
+Based on the work of Michael Herf : "fast floating-point radix sort"
+Avaliable on http://www.stereopsis.com/radix.html
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_memory.h"
+
+/*! \defgroup SORTING
+\brief
+Macros for sorting.
+*/
+
+//! Prototype for comparators
+class less_comparator
+{
+	public:
+
+	template<class T,class Z>
+	inline int operator() ( const T& a, const Z& b )
+	{
+		return ( a<b?-1:(a>b?1:0));
+	}
+};
+
+//! Prototype for comparators
+class integer_comparator
+{
+	public:
+
+	template<class T>
+	inline int operator() ( const T& a, const T& b )
+	{
+		return (int)(a-b);
+	}
+};
+
+//!Prototype for getting the integer representation of an object
+class uint_key_func
+{
+public:
+	template<class T>
+	inline GUINT operator()( const T& a)
+	{
+		return (GUINT)a;
+	}
+};
+
+
+//!Prototype for copying elements
+class copy_elements_func
+{
+public:
+	template<class T>
+	inline void operator()(T& a,T& b)
+	{
+		a = b;
+	}
+};
+
+//!Prototype for copying elements
+class memcopy_elements_func
+{
+public:
+	template<class T>
+	inline void operator()(T& a,T& b)
+	{
+		gim_simd_memcpy(&a,&b,sizeof(T));
+	}
+};
+
+
+//! @{
+struct GIM_RSORT_TOKEN
+{
+    GUINT m_key;
+    GUINT m_value;
+    GIM_RSORT_TOKEN()
+    {
+    }
+    GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
+    {
+    	m_key = rtoken.m_key;
+    	m_value = rtoken.m_value;
+    }
+
+    inline bool operator <(const GIM_RSORT_TOKEN& other) const
+	{
+		return (m_key < other.m_key);
+	}
+
+	inline bool operator >(const GIM_RSORT_TOKEN& other) const
+	{
+		return (m_key > other.m_key);
+	}
+};
+
+//! Prototype for comparators
+class GIM_RSORT_TOKEN_COMPARATOR
+{
+	public:
+
+	inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
+	{
+		return (int)((a.m_key) - (b.m_key));
+	}
+};
+
+
+
+#define kHist 2048
+// ---- utils for accessing 11-bit quantities
+#define D11_0(x)	(x & 0x7FF)
+#define D11_1(x)	(x >> 11 & 0x7FF)
+#define D11_2(x)	(x >> 22 )
+
+
+
+///Radix sort for unsigned integer keys
+inline void gim_radix_sort_rtokens(
+				GIM_RSORT_TOKEN * array,
+				GIM_RSORT_TOKEN * sorted, GUINT element_count)
+{
+	GUINT i;
+	GUINT b0[kHist * 3];
+	GUINT *b1 = b0 + kHist;
+	GUINT *b2 = b1 + kHist;
+	for (i = 0; i < kHist * 3; ++i)
+	{
+		b0[i] = 0;
+	}
+	GUINT fi;
+	GUINT pos;
+	for (i = 0; i < element_count; ++i)
+	{
+	    fi = array[i].m_key;
+		b0[D11_0(fi)] ++;
+		b1[D11_1(fi)] ++;
+		b2[D11_2(fi)] ++;
+	}
+	{
+		GUINT sum0 = 0, sum1 = 0, sum2 = 0;
+		GUINT tsum;
+		for (i = 0; i < kHist; ++i)
+		{
+			tsum = b0[i] + sum0;
+			b0[i] = sum0 - 1;
+			sum0 = tsum;
+			tsum = b1[i] + sum1;
+			b1[i] = sum1 - 1;
+			sum1 = tsum;
+			tsum = b2[i] + sum2;
+			b2[i] = sum2 - 1;
+			sum2 = tsum;
+		}
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = array[i].m_key;
+		pos = D11_0(fi);
+		pos = ++b0[pos];
+		sorted[pos].m_key = array[i].m_key;
+		sorted[pos].m_value = array[i].m_value;
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = sorted[i].m_key;
+		pos = D11_1(fi);
+		pos = ++b1[pos];
+		array[pos].m_key = sorted[i].m_key;
+		array[pos].m_value = sorted[i].m_value;
+	}
+	for (i = 0; i < element_count; ++i)
+	{
+        fi = array[i].m_key;
+		pos = D11_2(fi);
+		pos = ++b2[pos];
+		sorted[pos].m_key = array[i].m_key;
+		sorted[pos].m_value = array[i].m_value;
+	}
+}
+
+
+
+
+/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
+/*!
+*\param array Array of elements to sort
+*\param sorted_tokens Tokens of sorted elements
+*\param element_count element count
+*\param uintkey_macro Functor which retrieves the integer representation of an array element
+*/
+template<typename T, class GETKEY_CLASS>
+void gim_radix_sort_array_tokens(
+			T* array ,
+			GIM_RSORT_TOKEN * sorted_tokens,
+			GUINT element_count,GETKEY_CLASS uintkey_macro)
+{
+	GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+    for (GUINT _i=0;_i<element_count;++_i)
+    {
+        _unsorted[_i].m_key = uintkey_macro(array[_i]);
+        _unsorted[_i].m_value = _i;
+    }
+    gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
+    gim_free(_unsorted);
+    gim_free(_unsorted);
+}
+
+/// Sorts array in place. For generic use
+/*!
+\param type Type of the array
+\param array
+\param element_count
+\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
+\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
+*/
+template<typename T, class GETKEY_CLASS, class COPY_CLASS>
+void gim_radix_sort(
+	T * array, GUINT element_count,
+	GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
+{
+	GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN  *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
+    gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
+    T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
+    gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
+    for (GUINT _i=0;_i<element_count;++_i)
+    {
+        copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
+    }
+    gim_free(_original_array);
+    gim_free(_sorted);
+}
+
+//! Failsafe Iterative binary search,
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending  index of the array
+\param _search_key Value to find
+\param _comp_macro macro for comparing elements
+\param _found If true the value has found. Boolean
+\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
+*/
+template<class T, typename KEYCLASS, typename COMP_CLASS>
+bool  gim_binary_search_ex(
+		const T* _array, GUINT _start_i,
+		GUINT _end_i,GUINT & _result_index,
+		const KEYCLASS & _search_key,
+		COMP_CLASS _comp_macro)
+{
+	GUINT _k;
+	int _comp_result;
+	GUINT _i = _start_i;
+	GUINT _j = _end_i+1;
+	while (_i < _j)
+	{
+		_k = (_j+_i-1)/2;
+		_comp_result = _comp_macro(_array[_k], _search_key);
+		if (_comp_result == 0)
+		{
+			_result_index = _k;
+			return true;
+		}
+		else if (_comp_result < 0)
+		{
+			_i = _k+1;
+		}
+		else
+		{
+			_j = _k;
+		}
+	}
+	_result_index = _i;
+	return false;
+}
+
+
+
+//! Failsafe Iterative binary search,Template version
+/*!
+If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
+\param _array
+\param _start_i the beginning of the array
+\param _end_i the ending  index of the array
+\param _search_key Value to find
+\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
+\return true if found, else false
+*/
+template<class T>
+bool gim_binary_search(
+	const T*_array,GUINT _start_i,
+	GUINT _end_i,const T & _search_key,
+	GUINT & _result_index)
+{
+	GUINT _i = _start_i;
+	GUINT _j = _end_i+1;
+	GUINT _k;
+	while(_i < _j)
+	{
+		_k = (_j+_i-1)/2;
+		if(_array[_k]==_search_key)
+		{
+			_result_index = _k;
+			return true;
+		}
+		else if (_array[_k]<_search_key)
+		{
+			_i = _k+1;
+		}
+		else
+		{
+			_j = _k;
+		}
+	}
+	_result_index = _i;
+	return false;
+}
+
+
+
+///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+template <typename T, typename COMP_CLASS>
+void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
+{
+	/*  PRE: a[k+1..N] is a heap */
+	/* POST:  a[k..N]  is a heap */
+
+	T temp = pArr[k - 1];
+	/* k has child(s) */
+	while (k <= n/2)
+	{
+		int child = 2*k;
+
+		if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
+		{
+			child++;
+		}
+		/* pick larger child */
+		if (CompareFunc(temp , pArr[child - 1])<0)
+		{
+			/* move child up */
+			pArr[k - 1] = pArr[child - 1];
+			k = child;
+		}
+		else
+		{
+			break;
+		}
+	}
+	pArr[k - 1] = temp;
+} /*downHeap*/
+
+
+template <typename T, typename COMP_CLASS>
+void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
+{
+	/* sort a[0..N-1],  N.B. 0 to N-1 */
+	GUINT k;
+	GUINT n = element_count;
+	for (k = n/2; k > 0; k--)
+	{
+		gim_down_heap(pArr, k, n, CompareFunc);
+	}
+
+	/* a[1..N] is now a heap */
+	while ( n>=2 )
+	{
+		gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
+		--n;
+		/* restore a[1..i-1] heap */
+		gim_down_heap(pArr, 1, n, CompareFunc);
+	}
+}
+
+
+
+//! @}
+#endif // GIM_RADIXSORT_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp	(revision 1963)
@@ -0,0 +1,640 @@
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_tri_collision.h"
+
+
+#define TRI_LOCAL_EPSILON 0.000001f
+#define MIN_EDGE_EDGE_DIS 0.00001f
+
+
+class _GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+	GREAL margin;	
+	btVector3 tu_vertices[3];
+	btVector3 tv_vertices[3];
+	btVector4 tu_plane;
+	btVector4 tv_plane;
+	btVector3 closest_point_u;
+	btVector3 closest_point_v;
+	btVector3 edge_edge_dir;
+	btVector3 distances;
+	GREAL du[4];
+	GREAL du0du1;
+	GREAL du0du2;
+	GREAL dv[4];
+	GREAL dv0dv1;
+	GREAL dv0dv2;	
+	btVector3 temp_points[MAX_TRI_CLIPPING];
+	btVector3 temp_points1[MAX_TRI_CLIPPING];
+	btVector3 contact_points[MAX_TRI_CLIPPING];
+	
+
+
+	//! if returns false, the faces are paralele
+	SIMD_FORCE_INLINE bool compute_intervals(
+					const GREAL &D0,
+					const GREAL &D1,
+					const GREAL &D2,
+					const GREAL &D0D1,
+					const GREAL &D0D2,
+					GREAL & scale_edge0,
+					GREAL & scale_edge1,
+					GUINT &edge_index0,
+					GUINT &edge_index1)
+	{
+		if(D0D1>0.0f)
+		{
+			/* here we know that D0D2<=0.0 */
+			/* that is D0, D1 are on the same side, D2 on the other or on the plane */
+			scale_edge0 = -D2/(D0-D2);
+			scale_edge1 = -D1/(D2-D1);
+			edge_index0 = 2;edge_index1 = 1;
+		}
+		else if(D0D2>0.0f)
+		{
+			/* here we know that d0d1<=0.0 */
+			scale_edge0 = -D0/(D1-D0);
+			scale_edge1 = -D1/(D2-D1);
+			edge_index0 = 0;edge_index1 = 1;
+		}
+		else if(D1*D2>0.0f || D0!=0.0f)
+		{
+			/* here we know that d0d1<=0.0 or that D0!=0.0 */
+			scale_edge0 = -D0/(D1-D0);
+			scale_edge1 = -D2/(D0-D2);
+			edge_index0 = 0 ;edge_index1 = 2;
+		}
+		else
+		{
+			return false;
+		}
+		return true;
+	}
+
+
+	//! clip triangle
+	/*!
+	*/
+	SIMD_FORCE_INLINE GUINT clip_triangle(
+		const btVector4 & tri_plane,
+		const btVector3 * tripoints,
+		const btVector3 * srcpoints,
+		btVector3 * clip_points)
+	{
+		// edge 0
+
+		btVector4 edgeplane;
+
+		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+
+		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points,clipped_count,temp_points1);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+
+		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points1,clipped_count,clip_points);
+
+		return clipped_count;
+
+
+		/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
+		GUINT i1 = (i0+1)%3;
+		// edge 0
+		btVector3 temp_points[MAX_TRI_CLIPPING];
+		btVector3 temp_points1[MAX_TRI_CLIPPING];
+
+		GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
+			0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
+			DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
+		
+		
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+			0,temp_points,clipped_count,temp_points1,
+			DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+		clipped_count = PLANE_CLIP_POLYGON_GENERIC(
+			0,temp_points1,clipped_count,clipped_points,
+			DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
+
+		return clipped_count;*/
+	}
+
+	SIMD_FORCE_INLINE void sort_isect(
+		GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
+	{
+		if(isect1<isect0)
+		{
+			//swap
+			GIM_SWAP_NUMBERS(isect0,isect1);
+			GIM_SWAP_NUMBERS(e0,e1);
+			btVector3 tmp = vec0;
+			vec0 = vec1;
+			vec1 = tmp;
+		}
+	}
+
+	//! Test verifying interval intersection with the direction between planes
+	/*!
+	\pre tv_plane and tu_plane must be set
+	\post
+	distances[2] is set with the distance
+	closest_point_u, closest_point_v, edge_edge_dir are set too
+	\return
+	- 0: faces are paralele
+	- 1: face U casts face V
+	- 2: face V casts face U
+	- 3: nearest edges
+	*/
+	SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
+	{
+		// Compute direction of intersection line
+		edge_edge_dir = tu_plane.cross(tv_plane);
+		GREAL Dlen;
+		VEC_LENGTH(edge_edge_dir,Dlen);
+
+		if(Dlen<0.0001)
+		{
+			return 0; //faces near paralele
+		}
+
+		edge_edge_dir*= 1/Dlen;//normalize
+
+
+		// Compute interval for triangle 1
+		GUINT tu_e0,tu_e1;//edge indices
+		GREAL tu_scale_e0,tu_scale_e1;//edge scale
+		if(!compute_intervals(du[0],du[1],du[2],
+			du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
+
+		// Compute interval for triangle 2
+		GUINT tv_e0,tv_e1;//edge indices
+		GREAL tv_scale_e0,tv_scale_e1;//edge scale
+
+		if(!compute_intervals(dv[0],dv[1],dv[2],
+			dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
+
+		//proyected vertices
+		btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
+		btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
+
+		btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
+		btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
+
+		//proyected intervals
+		GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
+		GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
+
+		sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
+		sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
+
+		const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
+		const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
+
+		if(midpoint_u<midpoint_v)
+		{
+			if(isect_u[1]>=isect_v[1]) // face U casts face V
+			{
+				return 1;
+			}
+			else if(isect_v[0]<=isect_u[0]) // face V casts face U
+			{
+				return 2;
+			}
+			// closest points
+			closest_point_u = up_e1;
+			closest_point_v = vp_e0;
+			// calc edges and separation
+
+			if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
+			{
+				SEGMENT_COLLISION(
+					tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
+					tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
+					closest_point_u,
+					closest_point_v);
+
+				edge_edge_dir = closest_point_u-closest_point_v;
+				VEC_LENGTH(edge_edge_dir,distances[2]);
+				edge_edge_dir *= 1.0f/distances[2];// normalize
+			}
+			else
+			{
+				distances[2] = isect_v[0]-isect_u[1];//distance negative
+				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+			}
+
+		}
+		else
+		{
+			if(isect_v[1]>=isect_u[1]) // face V casts face U
+			{
+				return 2;
+			}
+			else if(isect_u[0]<=isect_v[0]) // face U casts face V
+			{
+				return 1;
+			}
+			// closest points
+			closest_point_u = up_e0;
+			closest_point_v = vp_e1;
+			// calc edges and separation
+
+			if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
+			{
+				SEGMENT_COLLISION(
+					tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
+					tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
+					closest_point_u,
+					closest_point_v);
+
+				edge_edge_dir = closest_point_u-closest_point_v;
+				VEC_LENGTH(edge_edge_dir,distances[2]);
+				edge_edge_dir *= 1.0f/distances[2];// normalize
+			}
+			else
+			{
+				distances[2] = isect_u[0]-isect_v[1];//distance negative
+				//edge_edge_dir *= -1.0f; //normal pointing from V to U
+			}
+		}
+		return 3;
+	}
+
+
+	//! collides by two sides
+	SIMD_FORCE_INLINE bool triangle_collision(
+					const btVector3 & u0,
+					const btVector3 & u1,
+					const btVector3 & u2,
+					GREAL margin_u,
+					const btVector3 & v0,
+					const btVector3 & v1,
+					const btVector3 & v2,
+					GREAL margin_v,
+					GIM_TRIANGLE_CONTACT_DATA & contacts)
+	{
+
+		margin = margin_u + margin_v;
+
+		tu_vertices[0] = u0;
+		tu_vertices[1] = u1;
+		tu_vertices[2] = u2;
+
+		tv_vertices[0] = v0;
+		tv_vertices[1] = v1;
+		tv_vertices[2] = v2;
+
+		//create planes
+		// plane v vs U points
+
+		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
+
+		du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
+		du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
+		du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
+
+
+		du0du1 = du[0] * du[1];
+		du0du2 = du[0] * du[2];
+
+
+		if(du0du1>0.0f && du0du2>0.0f)	// same sign on all of them + not equal 0 ?
+		{
+			if(du[0]<0) //we need test behind the triangle plane
+			{
+				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = -distances[0];
+				if(distances[0]>margin) return false; //never intersect
+
+				//reorder triangle v
+				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+			}
+			else
+			{
+				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+				if(distances[0]>margin) return false; //never intersect
+			}
+		}
+		else
+		{
+			//Look if we need to invert the triangle
+			distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
+
+			if(distances[0]<0.0f)
+			{
+				//reorder triangle v
+				VEC_SWAP(tv_vertices[0],tv_vertices[1]);
+				VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
+
+				distances[0] = GIM_MAX3(du[0],du[1],du[2]);
+				distances[0] = -distances[0];
+			}
+			else
+			{
+				distances[0] = GIM_MIN3(du[0],du[1],du[2]);
+			}
+		}
+
+
+		// plane U vs V points
+
+		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
+
+		dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
+		dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
+		dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
+
+		dv0dv1 = dv[0] * dv[1];
+		dv0dv2 = dv[0] * dv[2];
+
+
+		if(dv0dv1>0.0f && dv0dv2>0.0f)	// same sign on all of them + not equal 0 ?
+		{
+			if(dv[0]<0) //we need test behind the triangle plane
+			{
+				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = -distances[1];
+				if(distances[1]>margin) return false; //never intersect
+
+				//reorder triangle u
+				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+			}
+			else
+			{
+				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+				if(distances[1]>margin) return false; //never intersect
+			}
+		}
+		else
+		{
+			//Look if we need to invert the triangle
+			distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
+
+			if(distances[1]<0.0f)
+			{
+				//reorder triangle v
+				VEC_SWAP(tu_vertices[0],tu_vertices[1]);
+				VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
+
+				distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
+				distances[1] = -distances[1];
+			}
+			else
+			{
+				distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
+			}
+		}
+
+		GUINT bl;
+		/* bl = cross_line_intersection_test();
+		if(bl==3)
+		{
+			//take edge direction too
+			bl = distances.maxAxis();
+		}
+		else
+		{*/
+			bl = 0;
+			if(distances[0]<distances[1]) bl = 1;
+		//}
+
+		if(bl==2) //edge edge separation
+		{
+			if(distances[2]>margin) return false;
+
+			contacts.m_penetration_depth = -distances[2] + margin;
+			contacts.m_points[0] = closest_point_v;
+			contacts.m_point_count = 1;
+			VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
+
+			return true;
+		}
+
+		//clip face against other
+
+		
+		GUINT point_count;
+		//TODO
+		if(bl == 0) //clip U points against V
+		{
+			point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
+			if(point_count == 0) return false;						
+			contacts.merge_points(tv_plane,margin,contact_points,point_count);			
+		}
+		else //clip V points against U
+		{
+			point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
+			if(point_count == 0) return false;			
+			contacts.merge_points(tu_plane,margin,contact_points,point_count);
+			contacts.m_separating_normal *= -1.f;
+		}
+		if(contacts.m_point_count == 0) return false;
+		return true;
+	}
+
+};
+
+
+/*class _GIM_TRIANGLE_CALCULATION_CACHE
+{
+public:
+	GREAL margin;
+	GUINT clipped_count;
+	btVector3 tu_vertices[3];
+	btVector3 tv_vertices[3];
+	btVector3 temp_points[MAX_TRI_CLIPPING];
+	btVector3 temp_points1[MAX_TRI_CLIPPING];
+	btVector3 clipped_points[MAX_TRI_CLIPPING];
+	GIM_TRIANGLE_CONTACT_DATA contacts1;
+	GIM_TRIANGLE_CONTACT_DATA contacts2;
+
+
+	//! clip triangle
+	GUINT clip_triangle(
+		const btVector4 & tri_plane,
+		const btVector3 * tripoints,
+		const btVector3 * srcpoints,
+		btVector3 * clipped_points)
+	{
+		// edge 0
+
+		btVector4 edgeplane;
+
+		EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
+
+		GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
+			edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 1
+
+		EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points,clipped_count,temp_points1);
+
+		if(clipped_count == 0) return 0;
+
+		// edge 2
+
+		EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
+
+		clipped_count = PLANE_CLIP_POLYGON3D(
+			edgeplane,temp_points1,clipped_count,clipped_points);
+
+		return clipped_count;
+	}
+
+
+
+
+	//! collides only on one side
+	bool triangle_collision(
+					const btVector3 & u0,
+					const btVector3 & u1,
+					const btVector3 & u2,
+					GREAL margin_u,
+					const btVector3 & v0,
+					const btVector3 & v1,
+					const btVector3 & v2,
+					GREAL margin_v,
+					GIM_TRIANGLE_CONTACT_DATA & contacts)
+	{
+
+		margin = margin_u + margin_v;
+
+		
+		tu_vertices[0] = u0;
+		tu_vertices[1] = u1;
+		tu_vertices[2] = u2;
+
+		tv_vertices[0] = v0;
+		tv_vertices[1] = v1;
+		tv_vertices[2] = v2;
+
+		//create planes
+		// plane v vs U points
+
+
+		TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
+
+		clipped_count = clip_triangle(
+			contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
+
+		if(clipped_count == 0 )
+		{
+			 return false;//Reject
+		}
+
+		//find most deep interval face1
+		contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
+		if(contacts1.m_point_count == 0) return false; // too far
+
+		//Normal pointing to triangle1
+		//contacts1.m_separating_normal *= -1.f;
+
+		//Clip tri1 by tri2 edges
+
+		TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
+
+		clipped_count = clip_triangle(
+			contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
+
+		if(clipped_count == 0 )
+		{
+			 return false;//Reject
+		}
+
+		//find most deep interval face1
+		contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
+		if(contacts2.m_point_count == 0) return false; // too far
+
+		contacts2.m_separating_normal *= -1.f;
+
+		////check most dir for contacts
+		if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
+		{
+			contacts.copy_from(contacts2);
+		}
+		else
+		{
+			contacts.copy_from(contacts1);
+		}
+		return true;
+	}
+
+
+};*/
+
+
+
+bool GIM_TRIANGLE::collide_triangle_hard_test(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+{
+	_GIM_TRIANGLE_CALCULATION_CACHE calc_cache;	
+	return calc_cache.triangle_collision(
+					m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
+					other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
+					contact_data);
+
+}
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h	(revision 1963)
@@ -0,0 +1,383 @@
+#ifndef GIM_TRI_COLLISION_H_INCLUDED
+#define GIM_TRI_COLLISION_H_INCLUDED
+
+/*! \file gim_tri_collision.h
+\author Francisco Len Nßjera
+*/
+/*
+-----------------------------------------------------------------------------
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of EITHER:
+   (1) The GNU Lesser General Public License as published by the Free
+       Software Foundation; either version 2.1 of the License, or (at
+       your option) any later version. The text of the GNU Lesser
+       General Public License is included with this library in the
+       file GIMPACT-LICENSE-LGPL.TXT.
+   (2) The BSD-style license that is included with this library in
+       the file GIMPACT-LICENSE-BSD.TXT.
+   (3) The zlib/libpng license that is included with this library in
+       the file GIMPACT-LICENSE-ZLIB.TXT.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
+ GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
+
+-----------------------------------------------------------------------------
+*/
+
+#include "gim_box_collision.h"
+#include "gim_clip_polygon.h"
+
+/*! \addtogroup GEOMETRIC_OPERATIONS
+*/
+//! @{
+
+
+
+#define MAX_TRI_CLIPPING 16
+
+//! Structure for collision
+struct GIM_TRIANGLE_CONTACT_DATA
+{
+    GREAL m_penetration_depth;
+    GUINT m_point_count;
+    btVector4 m_separating_normal;
+    btVector3 m_points[MAX_TRI_CLIPPING];
+
+	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
+	{
+		m_penetration_depth = other.m_penetration_depth;
+		m_separating_normal = other.m_separating_normal;
+		m_point_count = other.m_point_count;
+		GUINT i = m_point_count;
+		while(i--)
+		{
+			m_points[i] = other.m_points[i];
+		}
+	}
+
+	GIM_TRIANGLE_CONTACT_DATA()
+	{
+	}
+
+	GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
+	{
+		copy_from(other);
+	}
+
+	
+	
+
+    //! classify points that are closer
+    template<typename DISTANCE_FUNC,typename CLASS_PLANE>
+    SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
+    				GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
+    {	
+    	m_point_count = 0;
+    	m_penetration_depth= -1000.0f;
+
+		GUINT point_indices[MAX_TRI_CLIPPING];
+
+		GUINT _k;
+
+		for(_k=0;_k<point_count;_k++)
+		{
+			GREAL _dist = -distance_func(plane,points[_k]) + margin;
+
+			if(_dist>=0.0f)
+			{
+				if(_dist>m_penetration_depth)
+				{
+					m_penetration_depth = _dist;
+					point_indices[0] = _k;
+					m_point_count=1;
+				}
+				else if((_dist+G_EPSILON)>=m_penetration_depth)
+				{
+					point_indices[m_point_count] = _k;
+					m_point_count++;
+				}
+			}
+		}
+
+		for( _k=0;_k<m_point_count;_k++)
+		{
+			m_points[_k] = points[point_indices[_k]];
+		}
+	}
+
+	//! classify points that are closer
+	SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
+										 const btVector3 * points, GUINT point_count)
+	{
+		m_separating_normal = plane;
+		mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
+	}
+};
+
+
+//! Class for colliding triangles
+class GIM_TRIANGLE
+{
+public:
+	btScalar m_margin;
+    btVector3 m_vertices[3];
+
+    GIM_TRIANGLE():m_margin(0.1f)
+    {
+    }
+
+    SIMD_FORCE_INLINE GIM_AABB get_box()  const
+    {
+    	return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+    }
+
+    SIMD_FORCE_INLINE void get_normal(btVector3 &normal)  const
+    {
+    	TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
+    }
+
+    SIMD_FORCE_INLINE void get_plane(btVector4 &plane)  const
+    {
+    	TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
+    }
+
+    SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
+    {
+    	m_vertices[0] = trans(m_vertices[0]);
+    	m_vertices[1] = trans(m_vertices[1]);
+    	m_vertices[2] = trans(m_vertices[2]);
+    }
+
+    SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane)  const
+    {
+		const btVector3 & e0 = m_vertices[edge_index];
+		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
+		EDGE_PLANE(e0,e1,triangle_normal,plane);
+    }
+
+    //! Gets the relative transformation of this triangle
+    /*!
+    The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
+    - triangle normal corresponds to Z axis.
+    - 1st normalized edge corresponds to X axis,
+
+    */
+    SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform)  const
+    {
+    	btMatrix3x3 & matrix = triangle_transform.getBasis();
+
+    	btVector3 zaxis;
+    	get_normal(zaxis);
+    	MAT_SET_Z(matrix,zaxis);
+
+    	btVector3 xaxis = m_vertices[1] - m_vertices[0];
+    	VEC_NORMALIZE(xaxis);
+    	MAT_SET_X(matrix,xaxis);
+
+    	//y axis
+    	xaxis = zaxis.cross(xaxis);
+    	MAT_SET_Y(matrix,xaxis);
+
+    	triangle_transform.setOrigin(m_vertices[0]);
+    }
+
+
+	//! Test triangles by finding separating axis
+	/*!
+	\param other Triangle for collide
+	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+	*/
+	bool collide_triangle_hard_test(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
+
+	//! Test boxes before doing hard test
+	/*!
+	\param other Triangle for collide
+	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
+	\
+	*/
+	SIMD_FORCE_INLINE bool collide_triangle(
+		const GIM_TRIANGLE & other,
+		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
+	{
+		//test box collisioin
+		GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
+		GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
+		if(!boxu.has_collision(boxv)) return false;
+
+		//do hard test
+		return collide_triangle_hard_test(other,contact_data);
+	}
+
+	/*!
+
+	Solve the System for u,v parameters:
+
+	u*axe1[i1] + v*axe2[i1] = vecproj[i1]
+	u*axe1[i2] + v*axe2[i2] = vecproj[i2]
+
+	sustitute:
+	v = (vecproj[i2] - u*axe1[i2])/axe2[i2]
+
+	then the first equation in terms of 'u':
+
+	--> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1]
+
+	--> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1]
+
+	--> u*(axe1[i1]  - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2]
+
+	--> u*((axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2]
+
+	--> u*(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]
+
+	--> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])
+
+if 0.0<= u+v <=1.0 then they are inside of triangle
+
+	\return false if the point is outside of triangle.This function  doesn't take the margin
+	*/
+	SIMD_FORCE_INLINE bool get_uv_parameters(
+			const btVector3 & point,
+			const btVector3 & tri_plane,
+			GREAL & u, GREAL & v) const
+	{
+		btVector3 _axe1 = m_vertices[1]-m_vertices[0];
+		btVector3 _axe2 = m_vertices[2]-m_vertices[0];
+		btVector3 _vecproj = point - m_vertices[0];
+		GUINT _i1 = (tri_plane.closestAxis()+1)%3;
+		GUINT _i2 = (_i1+1)%3;
+		if(btFabs(_axe2[_i2])<G_EPSILON)
+		{
+			u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1]  - _axe1[_i1]*_axe2[_i2]);
+			v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
+		}
+		else
+		{
+			u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2]  - _axe1[_i2]*_axe2[_i1]);
+			v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
+		}
+
+		if(u<-G_EPSILON)
+		{
+			return false;
+		}
+		else if(v<-G_EPSILON)
+		{
+			return false;
+		}
+		else
+		{
+			float sumuv;
+			sumuv = u+v;
+			if(sumuv<-G_EPSILON)
+			{
+				return false;
+			}
+			else if(sumuv-1.0f>G_EPSILON)
+			{
+				return false;
+			}
+		}
+		return true;
+	}
+
+	//! is point in triangle beam?
+	/*!
+	Test if point is in triangle, with m_margin tolerance
+	*/
+	SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
+	{
+		//Test with edge 0
+		btVector4 edge_plane;
+		this->get_edge_plane(0,tri_normal,edge_plane);
+		GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+
+		this->get_edge_plane(1,tri_normal,edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+
+		this->get_edge_plane(2,tri_normal,edge_plane);
+		dist = DISTANCE_PLANE_POINT(edge_plane,point);
+		if(dist-m_margin>0.0f) return false; // outside plane
+		return true;
+	}
+
+
+	//! Bidireccional ray collision
+	SIMD_FORCE_INLINE bool ray_collision(
+		const btVector3 & vPoint,
+		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+	{
+		btVector4 faceplane;
+		{
+			btVector3 dif1 = m_vertices[1] - m_vertices[0];
+			btVector3 dif2 = m_vertices[2] - m_vertices[0];
+    		VEC_CROSS(faceplane,dif1,dif2);
+    		faceplane[3] = m_vertices[0].dot(faceplane);
+		}
+
+		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+		if(res == 0) return false;
+		if(! is_point_inside(pout,faceplane)) return false;
+
+		if(res==2) //invert normal
+		{
+			triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
+		}
+		else
+		{
+			triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+		}
+
+		VEC_NORMALIZE(triangle_normal);
+
+		return true;
+	}
+
+
+	//! one direccion ray collision
+	SIMD_FORCE_INLINE bool ray_collision_front_side(
+		const btVector3 & vPoint,
+		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
+		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
+	{
+		btVector4 faceplane;
+		{
+			btVector3 dif1 = m_vertices[1] - m_vertices[0];
+			btVector3 dif2 = m_vertices[2] - m_vertices[0];
+    		VEC_CROSS(faceplane,dif1,dif2);
+    		faceplane[3] = m_vertices[0].dot(faceplane);
+		}
+
+		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
+		if(res != 1) return false;
+
+		if(!is_point_inside(pout,faceplane)) return false;
+
+		triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
+
+		VEC_NORMALIZE(triangle_normal);
+
+		return true;
+	}
+
+};
+
+
+
+//! @}
+
+#endif // GIM_TRI_COLLISION_H_INCLUDED
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp	(revision 1963)
@@ -0,0 +1,225 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btContinuousConvexCollision.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+
+btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape*	convexA,const btConvexShape*	convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 64
+
+bool	btContinuousConvexCollision::calcTimeOfImpact(
+				const btTransform& fromA,
+				const btTransform& toA,
+				const btTransform& fromB,
+				const btTransform& toB,
+				CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	/// compute linear and angular velocity for this interval, to interpolate
+	btVector3 linVelA,angVelA,linVelB,angVelB;
+	btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
+	btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
+
+
+	btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
+	btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
+
+	btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+	btVector3 relLinVel = (linVelB-linVelA);
+
+	btScalar relLinVelocLength = (linVelB-linVelA).length();
+	
+	if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
+		return false;
+
+
+	btScalar radius = btScalar(0.001);
+
+	btScalar lambda = btScalar(0.);
+	btVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+
+	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	btTransform identityTrans;
+	identityTrans.setIdentity();
+
+	btSphereShape	raySphere(btScalar(0.0));
+	raySphere.setMargin(btScalar(0.));
+
+
+//	result.drawCoordSystem(sphereTr);
+
+	btPointCollector	pointCollector1;
+
+	{
+		
+		btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);		
+		btGjkPairDetector::ClosestPointInput input;
+	
+		//we don't use margins during CCD
+	//	gjk.setIgnoreMargin(true);
+
+		input.m_transformA = fromA;
+		input.m_transformB = fromB;
+		gjk.getClosestPoints(input,pointCollector1,0);
+
+		hasResult = pointCollector1.m_hasResult;
+		c = pointCollector1.m_pointInWorld;
+	}
+
+	if (hasResult)
+	{
+		btScalar dist;
+		dist = pointCollector1.m_distance;
+		n = pointCollector1.m_normalOnBInWorld;
+
+		btScalar projectedLinearVelocity = relLinVel.dot(n);
+		
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
+
+			projectedLinearVelocity = relLinVel.dot(n);
+
+			//calculate safe moving fraction from distance / (linear+rotational velocity)
+			
+			//btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
+			//btScalar clippedDist  = dist;
+			
+			
+			dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+			
+			
+			lambda = lambda + dLambda;
+
+			if (lambda > btScalar(1.))
+				return false;
+
+			if (lambda < btScalar(0.))
+				return false;
+
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
+				break;
+			}
+			lastLambda = lambda;
+
+			
+
+			//interpolate to next lambda
+			btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+			btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+			btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+			relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+			result.DebugDraw( lambda );
+
+			btPointCollector	pointCollector;
+			btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+			btGjkPairDetector::ClosestPointInput input;
+			input.m_transformA = interpolatedTransA;
+			input.m_transformB = interpolatedTransB;
+			gjk.getClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < btScalar(0.))
+				{
+					//degenerate ?!
+					result.m_fraction = lastLambda;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;//.setValue(1,1,1);// = n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
+			return false;
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		result.m_hitPoint = c;
+		return true;
+	}
+
+	return false;
+
+/*
+//todo:
+	//if movement away from normal, discard result
+	btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+	if (result.m_fraction < btScalar(1.))
+	{
+		if (move.dot(result.m_normal) <= btScalar(0.))
+		{
+		}
+	}
+*/
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h	(revision 1963)
@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+class btConvexShape;
+
+/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class btContinuousConvexCollision : public btConvexCast
+{
+	btSimplexSolverInterface* m_simplexSolver;
+	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+
+public:
+
+	btContinuousConvexCollision (const btConvexShape*	shapeA,const btConvexShape*	shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+	virtual bool	calcTimeOfImpact(
+				const btTransform& fromA,
+				const btTransform& toA,
+				const btTransform& fromB,
+				const btTransform& toB,
+				CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp	(revision 1963)
@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btConvexCast.h"
+
+btConvexCast::~btConvexCast()
+{
+}
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h	(revision 1963)
@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+class btMinkowskiSumShape;
+#include "LinearMath/btIDebugDraw.h"
+
+/// btConvexCast is an interface for Casting
+class btConvexCast
+{
+public:
+
+
+	virtual ~btConvexCast();
+
+	///RayResult stores the closest result
+	/// alternatively, add a callback method to decide about closest/all results
+	struct	CastResult
+	{
+		//virtual bool	addRayResult(const btVector3& normal,btScalar	fraction) = 0;
+				
+		virtual void	DebugDraw(btScalar	fraction) {(void)fraction;}
+		virtual void	drawCoordSystem(const btTransform& trans) {(void)trans;}
+
+		CastResult()
+			:m_fraction(btScalar(1e30)),
+			m_debugDrawer(0),
+			m_allowedPenetration(btScalar(0))
+		{
+		}
+
+
+		virtual ~CastResult() {};
+
+		btTransform	m_hitTransformA;
+		btTransform	m_hitTransformB;
+		btVector3	m_normal;
+		btVector3   m_hitPoint;
+		btScalar	m_fraction; //input and output
+		btIDebugDraw* m_debugDrawer;
+		btScalar	m_allowedPenetration;
+
+	};
+
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h	(revision 1963)
@@ -0,0 +1,43 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+class btStackAlloc;
+class btVector3;
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+#include "LinearMath/btPoint3.h"
+class btTransform;
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class btConvexPenetrationDepthSolver
+{
+public:	
+	
+	virtual ~btConvexPenetrationDepthSolver() {};
+	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+		const btConvexShape* convexA,const btConvexShape* convexB,
+					const btTransform& transA,const btTransform& transB,
+				btVector3& v, btPoint3& pa, btPoint3& pb,
+				class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+				) = 0;
+
+
+};
+#endif //CONVEX_PENETRATION_DEPTH_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h	(revision 1963)
@@ -0,0 +1,88 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+class btStackAlloc;
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct btDiscreteCollisionDetectorInterface
+{
+	
+	struct Result
+	{
+	
+		virtual ~Result(){}	
+
+		///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)=0;
+		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
+	};
+
+	struct ClosestPointInput
+	{
+		ClosestPointInput()
+			:m_maximumDistanceSquared(btScalar(1e30)),
+			m_stackAlloc(0)
+		{
+		}
+
+		btTransform m_transformA;
+		btTransform m_transformB;
+		btScalar	m_maximumDistanceSquared;
+		btStackAlloc* m_stackAlloc;
+	};
+
+	virtual ~btDiscreteCollisionDetectorInterface() {};
+
+	//
+	// give either closest points (distance > 0) or penetration (distance)
+	// the normal always points from B towards A
+	//
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
+
+};
+
+struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
+{
+		btVector3	m_normalOnSurfaceB;
+		btVector3	m_closestPointInB;
+		btScalar	m_distance; //negative means penetration !
+
+		btStorageResult() : m_distance(btScalar(1e30))
+		{
+
+		}
+		virtual ~btStorageResult() {};
+
+		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		{
+			if (depth < m_distance)
+			{
+				m_normalOnSurfaceB = normalOnBInWorld;
+				m_closestPointInB = pointInWorld;
+				m_distance = depth;
+			}
+		}
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp	(revision 1963)
@@ -0,0 +1,176 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btGjkConvexCast.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+
+btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool	btGjkConvexCast::calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result)
+{
+
+
+	m_simplexSolver->reset();
+
+	/// compute linear velocity for this interval, to interpolate
+	//assume no rotation/angular velocity, assert here?
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
+
+	btScalar radius = btScalar(0.001);
+	btScalar lambda = btScalar(0.);
+	btVector3 v(1,0,0);
+
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+	btVector3 r = (linVelA-linVelB);
+
+	btScalar lastLambda = lambda;
+	//btScalar epsilon = btScalar(0.001);
+
+	int numIter = 0;
+	//first solution, using GJK
+
+
+	btTransform identityTrans;
+	identityTrans.setIdentity();
+
+
+//	result.drawCoordSystem(sphereTr);
+
+	btPointCollector	pointCollector;
+
+		
+	btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);		
+	btGjkPairDetector::ClosestPointInput input;
+
+	//we don't use margins during CCD
+	//	gjk.setIgnoreMargin(true);
+
+	input.m_transformA = fromA;
+	input.m_transformB = fromB;
+	gjk.getClosestPoints(input,pointCollector,0);
+
+	hasResult = pointCollector.m_hasResult;
+	c = pointCollector.m_pointInWorld;
+
+	if (hasResult)
+	{
+		btScalar dist;
+		dist = pointCollector.m_distance;
+		n = pointCollector.m_normalOnBInWorld;
+
+	
+
+		//not close enough
+		while (dist > radius)
+		{
+			numIter++;
+			if (numIter > maxIter)
+			{
+				return false; //todo: report a failure
+			}
+			btScalar dLambda = btScalar(0.);
+
+			btScalar projectedLinearVelocity = r.dot(n);
+			
+			dLambda = dist / (projectedLinearVelocity);
+
+			lambda = lambda - dLambda;
+
+			if (lambda > btScalar(1.))
+				return false;
+
+			if (lambda < btScalar(0.))
+				return false;
+
+			//todo: next check with relative epsilon
+			if (lambda <= lastLambda)
+			{
+				return false;
+				//n.setValue(0,0,0);
+				break;
+			}
+			lastLambda = lambda;
+
+			//interpolate to next lambda
+			result.DebugDraw( lambda );
+			input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+			input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+			
+			gjk.getClosestPoints(input,pointCollector,0);
+			if (pointCollector.m_hasResult)
+			{
+				if (pointCollector.m_distance < btScalar(0.))
+				{
+					result.m_fraction = lastLambda;
+					n = pointCollector.m_normalOnBInWorld;
+					result.m_normal=n;
+					result.m_hitPoint = pointCollector.m_pointInWorld;
+					return true;
+				}
+				c = pointCollector.m_pointInWorld;		
+				n = pointCollector.m_normalOnBInWorld;
+				dist = pointCollector.m_distance;
+			} else
+			{
+				//??
+				return false;
+			}
+
+		}
+
+		//is n normalized?
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if (n.dot(r)>=-result.m_allowedPenetration)
+			return false;
+
+		result.m_fraction = lambda;
+		result.m_normal = n;
+		result.m_hitPoint = c;
+		return true;
+	}
+
+	return false;
+
+
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h	(revision 1963)
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GJK_CONVEX_CAST_H
+#define GJK_CONVEX_CAST_H
+
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+#include "LinearMath/btVector3.h"
+#include "btConvexCast.h"
+class btConvexShape;
+class btMinkowskiSumShape;
+#include "btSimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class btGjkConvexCast : public btConvexCast
+{
+	btSimplexSolverInterface*	m_simplexSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+public:
+
+	btGjkConvexCast(const btConvexShape*	convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+
+	/// cast a convex against another convex object
+	virtual bool	calcTimeOfImpact(
+					const btTransform& fromA,
+					const btTransform& toA,
+					const btTransform& fromB,
+					const btTransform& toB,
+					CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp	(revision 1963)
@@ -0,0 +1,628 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+#include "btGjkEpa.h"
+#include <string.h> //for memset
+#include "LinearMath/btStackAlloc.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+namespace gjkepa_impl
+{
+
+//
+// Port. typedefs
+//
+
+typedef	btScalar		F;
+typedef bool			Z;
+typedef int				I;
+typedef unsigned int	U;
+typedef unsigned char	U1;
+typedef unsigned short	U2;
+
+typedef btVector3		Vector3;
+typedef btMatrix3x3		Rotation;
+
+//
+// Config
+//
+
+#if 0
+#define BTLOCALSUPPORT	localGetSupportingVertexWithoutMargin
+#else
+#define BTLOCALSUPPORT	localGetSupportingVertex
+#endif
+
+//
+// Const
+//
+
+
+#define cstInf				SIMD_INFINITY
+#define cstPi				SIMD_PI
+#define	cst2Pi				SIMD_2_PI
+#define GJK_maxiterations	(128)
+#define GJK_hashsize		(1<<6)
+#define GJK_hashmask		(GJK_hashsize-1)
+#define GJK_insimplex_eps	F(0.0001)
+#define GJK_sqinsimplex_eps	(GJK_insimplex_eps*GJK_insimplex_eps)
+#define EPA_maxiterations	256
+#define	EPA_inface_eps		F(0.01)
+#define EPA_accuracy		F(0.001)
+
+//
+// Utils
+//
+
+static inline F								Abs(F v)					{ return(v<0?-v:v); }
+static inline F								Sign(F v)					{ return(F(v<0?-1:1)); }
+template <typename T> static inline void	Swap(T& a,T& b)				{ T 
+t(a);a=b;b=t; }
+template <typename T> static inline T		Min(const T& a,const T& b)	{ 
+return(a<b?a:b); }
+template <typename T> static inline T		Max(const T& a,const T& b)	{ 
+return(a>b?a:b); }
+static inline void							ClearMemory(void* p,U sz)	{ memset(p,0,(size_t)sz); 
+}
+#if 0
+template <typename T> static inline void	Raise(const T& object)		{
+throw(object); }
+#else
+template <typename T> static inline void	Raise(const T&)				{}
+#endif
+
+
+
+//
+// GJK
+//
+struct	GJK
+	{
+	struct Mkv
+		{
+		Vector3	w;		/* Minkowski vertice	*/
+		Vector3	r;		/* Ray					*/
+		};
+	struct He
+		{
+		Vector3	v;
+		He*		n;
+		};
+	btStackAlloc*				sa;
+	btBlock*		sablock;
+	He*						table[GJK_hashsize];
+	Rotation				wrotations[2];
+	Vector3					positions[2];
+	const btConvexShape*	shapes[2];
+	Mkv						simplex[5];
+	Vector3					ray;
+	U						order;
+	U						iterations;
+	F						margin;
+	Z						failed;
+	//
+					GJK(btStackAlloc* psa,
+						const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
+						const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
+						F pmargin=0)
+		{
+		wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
+		wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
+		sa		=psa;
+		sablock	=sa->beginBlock();
+		margin	=pmargin;
+		failed	=false;
+		}
+	//
+					~GJK()
+		{
+		sa->endBlock(sablock);
+		}
+	// vdh : very dumm hash
+	static inline U	Hash(const Vector3& v)
+		{
+		//this doesn't compile under GCC 3.3.5, so add the ()...
+		//const U	h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473));
+		//return(((*((const U*)&h))*169639)&GJK_hashmask);
+	    const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473));
+	    return(((*((const U*)&h))*169639)&GJK_hashmask);
+		}
+	//
+	inline Vector3	LocalSupport(const Vector3& d,U i) const
+		{
+		return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]);
+		}
+	//
+	inline void		Support(const Vector3& d,Mkv& v) const
+		{
+		v.r	=	d;
+		v.w	=	LocalSupport(d,0)-LocalSupport(-d,1)+d*margin;
+		}
+	#define SPX(_i_)	simplex[_i_]
+	#define SPXW(_i_)	simplex[_i_].w
+	//
+	inline Z		FetchSupport()
+		{
+		const U			h(Hash(ray));
+		He*				e = (He*)(table[h]);
+		while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
+		e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
+		Support(ray,simplex[++order]);
+		return(ray.dot(SPXW(order))>0);
+		}
+	//
+	inline Z		SolveSimplex2(const Vector3& ao,const Vector3& ab)
+		{
+		if(ab.dot(ao)>=0)
+			{
+			const Vector3	cabo(cross(ab,ao));
+			if(cabo.length2()>GJK_sqinsimplex_eps)
+				{ ray=cross(cabo,ab); }
+				else
+				{ return(true); }
+			}
+			else
+			{ order=0;SPX(0)=SPX(1);ray=ao;	}
+		return(false);
+		}
+	//
+	inline Z		SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac)
+		{
+		return(SolveSimplex3a(ao,ab,ac,cross(ab,ac)));
+		}
+	//
+	inline Z		SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac,const Vector3& cabc)
+		{
+				if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps)
+			{ order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab));	}
+		else	if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps)
+			{ order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); }
+		else
+			{
+			const F			d(cabc.dot(ao));
+			if(Abs(d)>GJK_insimplex_eps)
+				{
+				if(d>0)
+					{ ray=cabc; }
+					else
+					{ ray=-cabc;Swap(SPX(0),SPX(1)); }
+				return(false);
+				} else return(true);
+			}
+		}
+	//
+	inline Z		SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& 
+ac,const Vector3& ad)
+		{
+		Vector3			crs;
+				if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps)
+			{ 
+order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); 
+}
+		else	if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps)
+			{ order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); }
+		else	if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps)
+			{ 
+order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); 
+}
+		else	return(true);
+		}
+	//
+	inline Z		SearchOrigin(const Vector3& initray=Vector3(1,0,0))
+		{
+		iterations	=	0;
+		order		=	(U)-1;
+		failed		=	false;
+		ray			=	initray.normalized();
+		ClearMemory(table,sizeof(void*)*GJK_hashsize);
+		FetchSupport();
+		ray			=	-SPXW(0);
+		for(;iterations<GJK_maxiterations;++iterations)
+			{
+			const F		rl(ray.length());
+			ray/=rl>0?rl:1;
+			if(FetchSupport())
+				{
+				Z	found(false);
+				switch(order)
+					{
+					case	1:	found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break;
+					case	2:	found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break;
+					case	3:	found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break;
+					}
+				if(found) return(true);
+				} else return(false);
+			}
+		failed=true;
+		return(false);
+		}
+	//
+	inline Z		EncloseOrigin()
+		{
+		switch(order)
+			{
+			/* Point		*/
+			case	0:	break;
+			/* Line			*/
+			case	1:
+				{
+				const Vector3	ab(SPXW(1)-SPXW(0));
+				const Vector3	b[]={	cross(ab,Vector3(1,0,0)),
+										cross(ab,Vector3(0,1,0)),
+										cross(ab,Vector3(0,0,1))};
+				const F			m[]={b[0].length2(),b[1].length2(),b[2].length2()};
+				const Rotation	r(btQuaternion(ab.normalized(),cst2Pi/3));
+				Vector3			w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]);
+				Support(w.normalized(),simplex[4]);w=r*w;
+				Support(w.normalized(),simplex[2]);w=r*w;
+				Support(w.normalized(),simplex[3]);w=r*w;
+				order=4;
+				return(true);
+				}
+			break;
+			/* Triangle		*/
+			case	2:
+				{
+				const 
+Vector3	n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized());
+				Support( n,simplex[3]);
+				Support(-n,simplex[4]);
+				order=4;
+				return(true);
+				}
+			break;
+			/* Tetrahedron	*/
+			case	3:	return(true);
+			/* Hexahedron	*/
+			case	4:	return(true);
+			}
+		return(false);
+		}
+	#undef SPX
+	#undef SPXW
+	};
+
+//
+// EPA
+//
+struct	EPA
+	{
+	//
+	struct Face
+		{
+		const GJK::Mkv*	v[3];
+		Face*			f[3];
+		U				e[3];
+		Vector3			n;
+		F				d;
+		U				mark;
+		Face*			prev;
+		Face*			next;
+		Face()			{}
+		};
+	//
+	GJK*			gjk;
+	btStackAlloc*		sa;
+	Face*			root;
+	U				nfaces;
+	U				iterations;
+	Vector3			features[2][3];
+	Vector3			nearest[2];
+	Vector3			normal;
+	F				depth;
+	Z				failed;
+	//
+					EPA(GJK* pgjk)
+		{
+		gjk		=	pgjk;
+		sa		=	pgjk->sa;
+		}
+	//
+					~EPA()
+		{
+		}
+	//
+	inline Vector3	GetCoordinates(const Face* face) const
+		{
+		const Vector3	o(face->n*-face->d);
+		const F			a[]={	cross(face->v[0]->w-o,face->v[1]->w-o).length(),
+								cross(face->v[1]->w-o,face->v[2]->w-o).length(),
+								cross(face->v[2]->w-o,face->v[0]->w-o).length()};
+		const F			sm(a[0]+a[1]+a[2]);
+		return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1));
+		}
+	//
+	inline Face*	FindBest() const
+		{
+		Face*	bf = 0;
+		if(root)
+			{
+			Face*	cf = root;
+			F		bd(cstInf);
+			do	{
+				if(cf->d<bd) { bd=cf->d;bf=cf; }
+				} while(0!=(cf=cf->next));
+			}
+		return(bf);
+		}
+	//
+	inline Z		Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv*
+c) const
+		{
+		const Vector3	nrm(cross(b->w-a->w,c->w-a->w));
+		const F			len(nrm.length());
+		const Z			valid(	(cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&&
+								(cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&&
+								(cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps));
+		f->v[0]	=	a;
+		f->v[1]	=	b;
+		f->v[2]	=	c;
+		f->mark	=	0;
+		f->n	=	nrm/(len>0?len:cstInf);
+		f->d	=	Max<F>(0,-f->n.dot(a->w));
+		return(valid);
+		}
+	//
+	inline Face*	NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
+		{
+		Face*	pf = (Face*)sa->allocate(sizeof(Face));
+		if(Set(pf,a,b,c))
+			{
+			if(root) root->prev=pf;
+			pf->prev=0;
+			pf->next=root;
+			root	=pf;
+			++nfaces;
+			}
+			else
+			{
+			pf->prev=pf->next=0;
+			}
+		return(pf);
+		}
+	//
+	inline void		Detach(Face* face)
+		{
+		if(face->prev||face->next)
+			{
+			--nfaces;
+			if(face==root)
+				{ root=face->next;root->prev=0; }
+				else
+				{
+				if(face->next==0)
+					{ face->prev->next=0; }
+					else
+					{ face->prev->next=face->next;face->next->prev=face->prev; }
+				}
+			face->prev=face->next=0;
+			}
+		}
+	//
+	inline void		Link(Face* f0,U e0,Face* f1,U e1) const
+		{
+		f0->f[e0]=f1;f1->e[e1]=e0;
+		f1->f[e1]=f0;f0->e[e0]=e1;
+		}
+	//
+	GJK::Mkv*		Support(const Vector3& w) const
+		{
+		GJK::Mkv*		v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));
+		gjk->Support(w,*v);
+		return(v);
+		}
+	//
+	U				BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& 
+ff)
+		{
+		static const U	mod3[]={0,1,2,0,1};
+		U				ne(0);
+		if(f.mark!=markid)
+			{
+			const U	e1(mod3[e+1]);
+			if((f.n.dot(w->w)+f.d)>0)
+				{
+				Face*	nf = NewFace(f.v[e1],f.v[e],w);
+				Link(nf,0,&f,e);
+				if(cf) Link(cf,1,nf,2); else ff=nf;
+				cf=nf;ne=1;
+				}
+				else
+				{
+				const U	e2(mod3[e+2]);
+				Detach(&f);
+				f.mark	=	markid;
+				ne		+=	BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff);
+				ne		+=	BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff);
+				}
+			}
+		return(ne);
+		}
+	//
+	inline F		EvaluatePD(F accuracy=EPA_accuracy)
+		{
+		btBlock*	sablock = sa->beginBlock();
+		Face*				bestface = 0;
+		U					markid(1);
+		depth		=	-cstInf;
+		normal		=	Vector3(0,0,0);
+		root		=	0;
+		nfaces		=	0;
+		iterations	=	0;
+		failed		=	false;
+		/* Prepare hull		*/
+		if(gjk->EncloseOrigin())
+			{
+			const U*		pfidx = 0;
+			U				nfidx= 0;
+			const U*		peidx = 0;
+			U				neidx = 0;
+			GJK::Mkv*		basemkv[5];
+			Face*			basefaces[6];
+			switch(gjk->order)
+				{
+				/* Tetrahedron		*/
+				case	3:	{
+							static const U	fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
+							static const 
+U	eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}};
+							pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6;
+							}	break;
+				/* Hexahedron		*/
+				case	4:	{
+							static const 
+U	fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
+							static const 
+U	eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}};
+							pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9;
+							}	break;
+				}
+			U i;
+
+			for( i=0;i<=gjk->order;++i)		{ 
+basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; 
+}
+			for( i=0;i<nfidx;++i,pfidx+=3)		{ 
+basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); 
+}
+			for( i=0;i<neidx;++i,peidx+=4)		{ 
+Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
+			}
+		if(0==nfaces)
+			{
+			sa->endBlock(sablock);
+			return(depth);
+			}
+		/* Expand hull		*/
+		for(;iterations<EPA_maxiterations;++iterations)
+			{
+			Face*		bf = FindBest();
+			if(bf)
+				{
+				GJK::Mkv*	w = Support(-bf->n);
+				const F		d(bf->n.dot(w->w)+bf->d);
+				bestface	=	bf;
+				if(d<-accuracy)
+					{
+					Face*	cf =0;
+					Face*	ff =0;
+					U		nf = 0;
+					Detach(bf);
+					bf->mark=++markid;
+					for(U i=0;i<3;++i)	{ 
+nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
+					if(nf<=2)			{ break; }
+					Link(cf,1,ff,2);
+					} else break;
+				} else break;
+			}
+		/* Extract contact	*/
+		if(bestface)
+			{
+			const Vector3	b(GetCoordinates(bestface));
+			normal			=	bestface->n;
+			depth			=	Max<F>(0,bestface->d);
+			for(U i=0;i<2;++i)
+				{
+				const F	s(F(i?-1:1));
+				for(U j=0;j<3;++j)
+					{
+					features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i);
+					}
+				}
+			nearest[0]		=	features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
+			nearest[1]		=	features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
+			} else failed=true;
+		sa->endBlock(sablock);
+		return(depth);
+		}
+	};
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa_impl;
+
+
+
+//
+bool	btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0,
+								const btConvexShape *shape1,const btTransform &wtrs1,
+								btScalar	radialmargin,
+								btStackAlloc* stackAlloc,
+								sResults&	results)
+{
+	
+
+/* Initialize					*/
+results.witnesses[0]	=
+results.witnesses[1]	=
+results.normal			=	Vector3(0,0,0);
+results.depth			=	0;
+results.status			=	sResults::Separated;
+results.epa_iterations	=	0;
+results.gjk_iterations	=	0;
+/* Use GJK to locate origin		*/
+GJK			gjk(stackAlloc,
+				wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
+				wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
+				radialmargin+EPA_accuracy);
+const Z		collide(gjk.SearchOrigin());
+results.gjk_iterations	=	static_cast<int>(gjk.iterations+1);
+if(collide)
+	{
+	/* Then EPA for penetration depth	*/
+	EPA			epa(&gjk);
+	const F		pd(epa.EvaluatePD());
+	results.epa_iterations	=	static_cast<int>(epa.iterations+1);
+	if(pd>0)
+		{
+		results.status			=	sResults::Penetrating;
+		results.normal			=	epa.normal;
+		results.depth			=	pd;
+		results.witnesses[0]	=	epa.nearest[0];
+		results.witnesses[1]	=	epa.nearest[1];
+		return(true);
+		} else { if(epa.failed) results.status=sResults::EPA_Failed; }
+	} else { if(gjk.failed) results.status=sResults::GJK_Failed; }
+return(false);
+}
+
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa.h	(revision 1963)
@@ -0,0 +1,53 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+
+#ifndef _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
+#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+class btStackAlloc;
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	btGjkEpaSolver
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	btScalar	depth;
+	int	epa_iterations;
+	int	gjk_iterations;
+	};
+static bool	Collide(const btConvexShape* shape0,const btTransform& wtrs0,
+					const btConvexShape* shape1,const btTransform& wtrs1,
+					btScalar	radialmargin,
+					btStackAlloc* stackAlloc,
+					sResults&	results);
+};
+
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp	(revision 1963)
@@ -0,0 +1,943 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "btGjkEpa2.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+namespace gjkepa2_impl
+{
+
+// Config
+
+	/* GJK	*/ 
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((btScalar)0.0001)
+#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS	((btScalar)0.0)
+
+	/* EPA	*/ 
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((btScalar)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((btScalar)0.00001)
+#define EPA_INSIDE_EPS		((btScalar)0.01)
+
+
+// Shorthands
+typedef unsigned int	U;
+typedef unsigned char	U1;
+
+// MinkowskiDiff
+struct	MinkowskiDiff
+	{
+	const btConvexShape*	m_shapes[2];
+	btMatrix3x3				m_toshape1;
+	btTransform				m_toshape0;
+	btVector3				(btConvexShape::*Ls)(const btVector3&) const;
+	void					EnableMargin(bool enable)
+		{
+		if(enable)
+			Ls=&btConvexShape::localGetSupportVertexNonVirtual;
+			else
+			Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
+		}	
+	inline btVector3		Support0(const btVector3& d) const
+		{
+		return(((m_shapes[0])->*(Ls))(d));
+		}
+	inline btVector3		Support1(const btVector3& d) const
+		{
+		return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
+		}
+	inline btVector3		Support(const btVector3& d) const
+		{
+		return(Support0(d)-Support1(-d));
+		}
+	btVector3				Support(const btVector3& d,U index) const
+		{
+		if(index)
+			return(Support1(d));
+			else
+			return(Support0(d));
+		}
+	};
+
+typedef	MinkowskiDiff	tShape;
+
+
+// GJK
+struct	GJK
+{
+/* Types		*/ 
+struct	sSV
+	{
+	btVector3	d,w;
+	};
+struct	sSimplex
+	{
+	sSV*		c[4];
+	btScalar	p[4];
+	U			rank;
+	};
+struct	eStatus	{ enum _ {
+	Valid,
+	Inside,
+	Failed		};};
+/* Fields		*/ 
+tShape			m_shape;
+btVector3		m_ray;
+btScalar		m_distance;
+sSimplex		m_simplices[2];
+sSV				m_store[4];
+sSV*			m_free[4];
+U				m_nfree;
+U				m_current;
+sSimplex*		m_simplex;
+eStatus::_		m_status;
+/* Methods		*/ 
+					GJK()
+	{
+	Initialize();
+	}
+void				Initialize()
+	{
+	m_ray		=	btVector3(0,0,0);
+	m_nfree		=	0;
+	m_status	=	eStatus::Failed;
+	m_current	=	0;
+	m_distance	=	0;
+	}
+eStatus::_			Evaluate(const tShape& shapearg,const btVector3& guess)
+	{
+	U			iterations=0;
+	btScalar	sqdist=0;
+	btScalar	alpha=0;
+	btVector3	lastw[4];
+	U			clastw=0;
+	/* Initialize solver		*/ 
+	m_free[0]			=	&m_store[0];
+	m_free[1]			=	&m_store[1];
+	m_free[2]			=	&m_store[2];
+	m_free[3]			=	&m_store[3];
+	m_nfree				=	4;
+	m_current			=	0;
+	m_status			=	eStatus::Valid;
+	m_shape				=	shapearg;
+	m_distance			=	0;
+	/* Initialize simplex		*/ 
+	m_simplices[0].rank	=	0;
+	m_ray				=	guess;
+	const btScalar	sqrl=	m_ray.length2();
+	appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
+	m_simplices[0].p[0]	=	1;
+	m_ray				=	m_simplices[0].c[0]->w;	
+	sqdist				=	sqrl;
+	lastw[0]			=
+	lastw[1]			=
+	lastw[2]			=
+	lastw[3]			=	m_ray;
+	/* Loop						*/ 
+	do	{
+		const U		next=1-m_current;
+		sSimplex&	cs=m_simplices[m_current];
+		sSimplex&	ns=m_simplices[next];
+		/* Check zero							*/ 
+		const btScalar	rl=m_ray.length();
+		if(rl<GJK_MIN_DISTANCE)
+			{/* Touching or inside				*/ 
+			m_status=eStatus::Inside;
+			break;
+			}
+		/* Append new vertice in -'v' direction	*/ 
+		appendvertice(cs,-m_ray);
+		const btVector3&	w=cs.c[cs.rank-1]->w;
+		bool				found=false;
+		for(U i=0;i<4;++i)
+			{
+			if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+				{ found=true;break; }
+			}
+		if(found)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+			else
+			{/* Update lastw					*/ 
+			lastw[clastw=(clastw+1)&3]=w;
+			}
+		/* Check for termination				*/ 
+		const btScalar	omega=dot(m_ray,w)/rl;
+		alpha=btMax(omega,alpha);
+		if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}		
+		/* Reduce simplex						*/ 
+		btScalar	weights[4];
+		U			mask=0;
+		switch(cs.rank)
+			{
+			case	2:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												weights,mask);break;
+			case	3:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												weights,mask);break;
+			case	4:	sqdist=projectorigin(	cs.c[0]->w,
+												cs.c[1]->w,
+												cs.c[2]->w,
+												cs.c[3]->w,
+												weights,mask);break;
+			}
+		if(sqdist>=0)
+			{/* Valid	*/ 
+			ns.rank		=	0;
+			m_ray		=	btVector3(0,0,0);
+			m_current	=	next;
+			for(U i=0,ni=cs.rank;i<ni;++i)
+				{
+				if(mask&(1<<i))
+					{
+					ns.c[ns.rank]		=	cs.c[i];
+					ns.p[ns.rank++]		=	weights[i];
+					m_ray				+=	cs.c[i]->w*weights[i];
+					}
+					else
+					{
+					m_free[m_nfree++]	=	cs.c[i];
+					}
+				}
+			if(mask==15) m_status=eStatus::Inside;
+			}
+			else
+			{/* Return old simplex				*/ 
+			removevertice(m_simplices[m_current]);
+			break;
+			}
+		m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
+		} while(m_status==eStatus::Valid);
+	m_simplex=&m_simplices[m_current];
+	switch(m_status)
+		{
+		case	eStatus::Valid:		m_distance=m_ray.length();break;
+		case	eStatus::Inside:	m_distance=0;break;
+		}	
+	return(m_status);
+	}
+bool					EncloseOrigin()
+	{
+	switch(m_simplex->rank)
+		{
+		case	1:
+			{
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				appendvertice(*m_simplex, axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-axis);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	2:
+			{
+			const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+			for(U i=0;i<3;++i)
+				{
+				btVector3		axis=btVector3(0,0,0);
+				axis[i]=1;
+				const btVector3	p=cross(d,axis);
+				if(p.length2()>0)
+					{
+					appendvertice(*m_simplex, p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					appendvertice(*m_simplex,-p);
+					if(EncloseOrigin())	return(true);
+					removevertice(*m_simplex);
+					}
+				}
+			}
+		break;
+		case	3:
+			{
+			const btVector3	n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+									m_simplex->c[2]->w-m_simplex->c[0]->w);
+			if(n.length2()>0)
+				{
+				appendvertice(*m_simplex,n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				appendvertice(*m_simplex,-n);
+				if(EncloseOrigin())	return(true);
+				removevertice(*m_simplex);
+				}
+			}
+		break;
+		case	4:
+			{
+			if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+							m_simplex->c[1]->w-m_simplex->c[3]->w,
+							m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+				return(true);
+			}
+		break;
+		}
+	return(false);
+	}
+/* Internals	*/ 
+void				getsupport(const btVector3& d,sSV& sv) const
+	{
+	sv.d	=	d/d.length();
+	sv.w	=	m_shape.Support(sv.d);
+	}
+void				removevertice(sSimplex& simplex)
+	{
+	m_free[m_nfree++]=simplex.c[--simplex.rank];
+	}
+void				appendvertice(sSimplex& simplex,const btVector3& v)
+	{
+	simplex.p[simplex.rank]=0;
+	simplex.c[simplex.rank]=m_free[--m_nfree];
+	getsupport(v,*simplex.c[simplex.rank++]);
+	}
+static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+	{
+	return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
+			a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
+			a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									btScalar* w,U& m)
+	{
+	const btVector3	d=b-a;
+	const btScalar	l=d.length2();
+	if(l>GJK_SIMPLEX2_EPS)
+		{
+		const btScalar	t(l>0?-dot(a,d)/l:0);
+		if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
+		else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
+		else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c};
+	const btVector3		dl[]={a-b,b-c,c-a};
+	const btVector3		n=cross(dl[0],dl[1]);
+	const btScalar		l=n.length2();
+	if(l>GJK_SIMPLEX3_EPS)
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[2];
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			if(dot(*vt[i],cross(dl[i],n))>0)
+				{
+				const U			j=imd3[i];
+				const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;				
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			const btScalar	d=dot(a,n);	
+			const btScalar	s=btSqrt(l);
+			const btVector3	p=n*(d/l);
+			mindist	=	p.length2();
+			m		=	7;
+			w[0]	=	(cross(dl[1],b-p)).length()/s;
+			w[1]	=	(cross(dl[2],c-p)).length()/s;
+			w[2]	=	1-(w[0]+w[1]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+static btScalar		projectorigin(	const btVector3& a,
+									const btVector3& b,
+									const btVector3& c,
+									const btVector3& d,
+									btScalar* w,U& m)
+	{
+	static const U		imd3[]={1,2,0};
+	const btVector3*	vt[]={&a,&b,&c,&d};
+	const btVector3		dl[]={a-d,b-d,c-d};
+	const btScalar		vl=det(dl[0],dl[1],dl[2]);
+	const bool			ng=(vl*dot(a,cross(b-c,a-b)))<=0;
+	if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+		{
+		btScalar	mindist=-1;
+		btScalar	subw[3];
+		U			subm;
+		for(U i=0;i<3;++i)
+			{
+			const U			j=imd3[i];
+			const btScalar	s=vl*dot(d,cross(dl[i],dl[j]));
+			if(s>0)
+				{
+				const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+				if((mindist<0)||(subd<mindist))
+					{
+					mindist		=	subd;
+					m			=	static_cast<U>((subm&1?1<<i:0)+
+									(subm&2?1<<j:0)+
+									(subm&4?8:0));
+					w[i]		=	subw[0];
+					w[j]		=	subw[1];
+					w[imd3[j]]	=	0;
+					w[3]		=	subw[2];
+					}
+				}
+			}
+		if(mindist<0)
+			{
+			mindist	=	0;
+			m		=	15;
+			w[0]	=	det(c,b,d)/vl;
+			w[1]	=	det(a,c,d)/vl;
+			w[2]	=	det(b,a,d)/vl;
+			w[3]	=	1-(w[0]+w[1]+w[2]);
+			}
+		return(mindist);
+		}
+	return(-1);
+	}
+};
+
+// EPA
+struct	EPA
+{
+/* Types		*/ 
+typedef	GJK::sSV	sSV;
+struct	sFace
+	{
+	btVector3	n;
+	btScalar	d;
+	btScalar	p;
+	sSV*		c[3];
+	sFace*		f[3];
+	sFace*		l[2];
+	U1			e[3];
+	U1			pass;
+	};
+struct	sList
+	{
+	sFace*		root;
+	U			count;
+				sList() : root(0),count(0)	{}
+	};
+struct	sHorizon
+	{
+	sFace*		cf;
+	sFace*		ff;
+	U			nf;
+				sHorizon() : cf(0),ff(0),nf(0)	{}
+	};
+struct	eStatus { enum _ {
+	Valid,
+	Touching,
+	Degenerated,
+	NonConvex,
+	InvalidHull,		
+	OutOfFaces,
+	OutOfVertices,
+	AccuraryReached,
+	FallBack,
+	Failed		};};
+/* Fields		*/ 
+eStatus::_		m_status;
+GJK::sSimplex	m_result;
+btVector3		m_normal;
+btScalar		m_depth;
+sSV				m_sv_store[EPA_MAX_VERTICES];
+sFace			m_fc_store[EPA_MAX_FACES];
+U				m_nextsv;
+sList			m_hull;
+sList			m_stock;
+/* Methods		*/ 
+					EPA()
+	{
+	Initialize();	
+	}
+
+
+					static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+	{
+	fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+	fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+	}
+static inline void		append(sList& list,sFace* face)
+	{
+	face->l[0]	=	0;
+	face->l[1]	=	list.root;
+	if(list.root) list.root->l[0]=face;
+	list.root	=	face;
+	++list.count;
+	}
+static inline void		remove(sList& list,sFace* face)
+	{
+	if(face->l[1]) face->l[1]->l[0]=face->l[0];
+	if(face->l[0]) face->l[0]->l[1]=face->l[1];
+	if(face==list.root) list.root=face->l[1];
+	--list.count;
+	}
+
+
+void				Initialize()
+	{
+	m_status	=	eStatus::Failed;
+	m_normal	=	btVector3(0,0,0);
+	m_depth		=	0;
+	m_nextsv	=	0;
+	for(U i=0;i<EPA_MAX_FACES;++i)
+		{
+		append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+		}
+	}
+eStatus::_			Evaluate(GJK& gjk,const btVector3& guess)
+	{
+	GJK::sSimplex&	simplex=*gjk.m_simplex;
+	if((simplex.rank>1)&&gjk.EncloseOrigin())
+		{
+
+		/* Clean up				*/ 
+		while(m_hull.root)
+			{
+			sFace*	f = m_hull.root;
+			remove(m_hull,f);
+			append(m_stock,f);
+			}
+		m_status	=	eStatus::Valid;
+		m_nextsv	=	0;
+		/* Orient simplex		*/ 
+		if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+					simplex.c[1]->w-simplex.c[3]->w,
+					simplex.c[2]->w-simplex.c[3]->w)<0)
+			{
+			btSwap(simplex.c[0],simplex.c[1]);
+			btSwap(simplex.p[0],simplex.p[1]);
+			}
+		/* Build initial hull	*/ 
+		sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+		if(m_hull.count==4)
+			{
+			sFace*		best=findbest();
+			sFace		outer=*best;
+			U			pass=0;
+			U			iterations=0;
+			bind(tetra[0],0,tetra[1],0);
+			bind(tetra[0],1,tetra[2],0);
+			bind(tetra[0],2,tetra[3],0);
+			bind(tetra[1],1,tetra[3],2);
+			bind(tetra[1],2,tetra[2],1);
+			bind(tetra[2],2,tetra[3],1);
+			m_status=eStatus::Valid;
+			for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+				{
+				if(m_nextsv<EPA_MAX_VERTICES)
+					{	
+					sHorizon		horizon;
+					sSV*			w=&m_sv_store[m_nextsv++];
+					bool			valid=true;					
+					best->pass	=	(U1)(++pass);
+					gjk.getsupport(best->n,*w);
+					const btScalar	wdist=dot(best->n,w->w)-best->d;
+					if(wdist>EPA_ACCURACY)
+						{
+						for(U j=0;(j<3)&&valid;++j)
+							{
+							valid&=expand(	pass,w,
+											best->f[j],best->e[j],
+											horizon);
+							}
+						if(valid&&(horizon.nf>=3))
+							{
+							bind(horizon.cf,1,horizon.ff,2);
+							remove(m_hull,best);
+							append(m_stock,best);
+							best=findbest();
+							if(best->p>=outer.p) outer=*best;
+							} else { m_status=eStatus::InvalidHull;break; }
+						} else { m_status=eStatus::AccuraryReached;break; }
+					} else { m_status=eStatus::OutOfVertices;break; }
+				}
+			const btVector3	projection=outer.n*outer.d;
+			m_normal	=	outer.n;
+			m_depth		=	outer.d;
+			m_result.rank	=	3;
+			m_result.c[0]	=	outer.c[0];
+			m_result.c[1]	=	outer.c[1];
+			m_result.c[2]	=	outer.c[2];
+			m_result.p[0]	=	cross(	outer.c[1]->w-projection,
+										outer.c[2]->w-projection).length();
+			m_result.p[1]	=	cross(	outer.c[2]->w-projection,
+										outer.c[0]->w-projection).length();
+			m_result.p[2]	=	cross(	outer.c[0]->w-projection,
+										outer.c[1]->w-projection).length();
+			const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+			m_result.p[0]	/=	sum;
+			m_result.p[1]	/=	sum;
+			m_result.p[2]	/=	sum;
+			return(m_status);
+			}
+		}
+	/* Fallback		*/ 
+	m_status	=	eStatus::FallBack;
+	m_normal	=	-guess;
+	const btScalar	nl=m_normal.length();
+	if(nl>0)
+		m_normal	=	m_normal/nl;
+		else
+		m_normal	=	btVector3(1,0,0);
+	m_depth	=	0;
+	m_result.rank=1;
+	m_result.c[0]=simplex.c[0];
+	m_result.p[0]=1;	
+	return(m_status);
+	}
+sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
+	{
+	if(m_stock.root)
+		{
+		sFace*	face=m_stock.root;
+		remove(m_stock,face);
+		append(m_hull,face);
+		face->pass	=	0;
+		face->c[0]	=	a;
+		face->c[1]	=	b;
+		face->c[2]	=	c;
+		face->n		=	cross(b->w-a->w,c->w-a->w);
+		const btScalar	l=face->n.length();
+		const bool		v=l>EPA_ACCURACY;
+		face->p		=	btMin(btMin(
+							dot(a->w,cross(face->n,a->w-b->w)),
+							dot(b->w,cross(face->n,b->w-c->w))),
+							dot(c->w,cross(face->n,c->w-a->w)))	/
+							(v?l:1);
+		face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
+		if(v)
+			{
+			face->d		=	dot(a->w,face->n)/l;
+			face->n		/=	l;
+			if(forced||(face->d>=-EPA_PLANE_EPS))
+				{
+				return(face);
+				} else m_status=eStatus::NonConvex;
+			} else m_status=eStatus::Degenerated;
+		remove(m_hull,face);
+		append(m_stock,face);
+		return(0);
+		}
+	m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
+	return(0);
+	}
+sFace*				findbest()
+	{
+	sFace*		minf=m_hull.root;
+	btScalar	mind=minf->d*minf->d;
+	btScalar	maxp=minf->p;
+	for(sFace* f=minf->l[1];f;f=f->l[1])
+		{
+		const btScalar	sqd=f->d*f->d;
+		if((f->p>=maxp)&&(sqd<mind))
+			{
+			minf=f;
+			mind=sqd;
+			maxp=f->p;
+			}
+		}
+	return(minf);
+	}
+bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
+	{
+	static const U	i1m3[]={1,2,0};
+	static const U	i2m3[]={2,0,1};
+	if(f->pass!=pass)
+		{
+		const U	e1=i1m3[e];
+		if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+			{
+			sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+			if(nf)
+				{
+				bind(nf,0,f,e);
+				if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+				horizon.cf=nf;
+				++horizon.nf;
+				return(true);
+				}
+			}
+			else
+			{
+			const U	e2=i2m3[e];
+			f->pass		=	(U1)pass;
+			if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+				expand(pass,w,f->f[e2],f->e[e2],horizon))
+				{
+				remove(m_hull,f);
+				append(m_stock,f);
+				return(true);
+				}
+			}
+		}
+	return(false);
+	}
+
+};
+
+//
+static void	Initialize(	const btConvexShape* shape0,const btTransform& wtrs0,
+						const btConvexShape* shape1,const btTransform& wtrs1,
+						btGjkEpaSolver2::sResults& results,
+						tShape& shape,
+						bool withmargins)
+{
+/* Results		*/ 
+results.witnesses[0]	=
+results.witnesses[1]	=	btVector3(0,0,0);
+results.status			=	btGjkEpaSolver2::sResults::Separated;
+/* Shape		*/ 
+shape.m_shapes[0]		=	shape0;
+shape.m_shapes[1]		=	shape1;
+shape.m_toshape1		=	wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
+shape.m_toshape0		=	wtrs0.inverseTimes(wtrs1);
+shape.EnableMargin(withmargins);
+}
+
+}
+
+//
+// Api
+//
+
+using namespace	gjkepa2_impl;
+
+//
+int			btGjkEpaSolver2::StackSizeRequirement()
+{
+return(sizeof(GJK)+sizeof(EPA));
+}
+
+//
+bool		btGjkEpaSolver2::Distance(	const btConvexShape*	shape0,
+										const btTransform&		wtrs0,
+										const btConvexShape*	shape1,
+										const btTransform&		wtrs1,
+										const btVector3&		guess,
+										sResults&				results)
+{
+tShape			shape;
+Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
+GJK				gjk;
+GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
+if(gjk_status==GJK::eStatus::Valid)
+	{
+	btVector3	w0=btVector3(0,0,0);
+	btVector3	w1=btVector3(0,0,0);
+	for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+		const btScalar	p=gjk.m_simplex->p[i];
+		w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+		w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+	results.witnesses[0]	=	wtrs0*w0;
+	results.witnesses[1]	=	wtrs0*w1;
+	results.normal			=	w0-w1;
+	results.distance		=	results.normal.length();
+	results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+	return(true);
+	}
+	else
+	{
+	results.status	=	gjk_status==GJK::eStatus::Inside?
+							sResults::Penetrating	:
+							sResults::GJK_Failed	;
+	return(false);
+	}
+}
+
+//
+bool	btGjkEpaSolver2::Penetration(	const btConvexShape*	shape0,
+										const btTransform&		wtrs0,
+										const btConvexShape*	shape1,
+										const btTransform&		wtrs1,
+										const btVector3&		guess,
+										sResults&				results,
+										bool					usemargins)
+{
+tShape			shape;
+Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
+GJK				gjk;	
+GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
+switch(gjk_status)
+	{
+	case	GJK::eStatus::Inside:
+		{
+		EPA				epa;
+		EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
+		if(epa_status!=EPA::eStatus::Failed)
+			{
+			btVector3	w0=btVector3(0,0,0);
+			for(U i=0;i<epa.m_result.rank;++i)
+				{
+				w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+				}
+			results.status			=	sResults::Penetrating;
+			results.witnesses[0]	=	wtrs0*w0;
+			results.witnesses[1]	=	wtrs0*(w0-epa.m_normal*epa.m_depth);
+			results.normal			=	-epa.m_normal;
+			results.distance		=	-epa.m_depth;
+			return(true);
+			} else results.status=sResults::EPA_Failed;
+		}
+	break;
+	case	GJK::eStatus::Failed:
+	results.status=sResults::GJK_Failed;
+	break;
+	}
+return(false);
+}
+
+//
+btScalar	btGjkEpaSolver2::SignedDistance(const btVector3& position,
+											btScalar margin,
+											const btConvexShape* shape0,
+											const btTransform& wtrs0,
+											sResults& results)
+{
+tShape			shape;
+btSphereShape	shape1(margin);
+btTransform		wtrs1(btQuaternion(0,0,0,1),position);
+Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
+GJK				gjk;	
+GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
+if(gjk_status==GJK::eStatus::Valid)
+	{
+	btVector3	w0=btVector3(0,0,0);
+	btVector3	w1=btVector3(0,0,0);
+	for(U i=0;i<gjk.m_simplex->rank;++i)
+		{
+		const btScalar	p=gjk.m_simplex->p[i];
+		w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+		w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+		}
+	results.witnesses[0]	=	wtrs0*w0;
+	results.witnesses[1]	=	wtrs0*w1;
+	const btVector3	delta=	results.witnesses[1]-
+							results.witnesses[0];
+	const btScalar	margin=	shape0->getMarginNonVirtual()+
+							shape1.getMarginNonVirtual();
+	const btScalar	length=	delta.length();	
+	results.normal			=	delta/length;
+	results.witnesses[0]	+=	results.normal*margin;
+	return(length-margin);
+	}
+	else
+	{
+	if(gjk_status==GJK::eStatus::Inside)
+		{
+		if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
+			{
+			const btVector3	delta=	results.witnesses[0]-
+									results.witnesses[1];
+			const btScalar	length=	delta.length();
+			if (length >= SIMD_EPSILON)
+				results.normal	=	delta/length;			
+			return(-length);
+			}
+		}	
+	}
+return(SIMD_INFINITY);
+}
+
+//
+bool	btGjkEpaSolver2::SignedDistance(const btConvexShape*	shape0,
+										const btTransform&		wtrs0,
+										const btConvexShape*	shape1,
+										const btTransform&		wtrs1,
+										const btVector3&		guess,
+										sResults&				results)
+{
+if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
+	return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
+	else
+	return(true);
+}
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h	(revision 1963)
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+GJK-EPA collision solver by Nathanael Presson, 2008
+*/
+#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+///btGjkEpaSolver contributed under zlib by Nathanael Presson
+struct	btGjkEpaSolver2
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	btScalar	distance;
+	};
+
+static int		StackSizeRequirement();
+
+static bool		Distance(	const btConvexShape* shape0,const btTransform& wtrs0,
+							const btConvexShape* shape1,const btTransform& wtrs1,
+							const btVector3& guess,
+							sResults& results);
+
+static bool		Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
+							const btConvexShape* shape1,const btTransform& wtrs1,
+							const btVector3& guess,
+							sResults& results,
+							bool usemargins=true);
+
+static btScalar	SignedDistance(	const btVector3& position,
+								btScalar margin,
+								const btConvexShape* shape,
+								const btTransform& wtrs,
+								sResults& results);
+							
+static bool		SignedDistance(	const btConvexShape* shape0,const btTransform& wtrs0,
+								const btConvexShape* shape1,const btTransform& wtrs1,
+								const btVector3& guess,
+								sResults& results);
+};
+
+#endif
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "btGjkEpaPenetrationDepthSolver.h"
+
+#ifndef __SPU__
+//#define USE_ORIGINAL_GJK 1
+#endif
+
+#ifdef USE_ORIGINAL_GJK
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#endif
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
+											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+											  const btTransform& transformA, const btTransform& transformB,
+											  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
+											  class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
+{
+
+	(void)debugDraw;
+	(void)v;
+	(void)simplexSolver;
+
+	const btScalar				radialmargin(btScalar(0.));
+	
+//#define USE_ORIGINAL_GJK 1
+#ifdef USE_ORIGINAL_GJK
+	btGjkEpaSolver::sResults	results;
+	if(btGjkEpaSolver::Collide(	pConvexA,transformA,
+								pConvexB,transformB,
+								radialmargin,stackAlloc,results))
+#else
+	btVector3	guessVector(transformA.getOrigin()-transformB.getOrigin());
+	btGjkEpaSolver2::sResults	results;
+	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
+								pConvexB,transformB,
+								guessVector,results))
+	
+#endif
+		{
+	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+		wWitnessOnA = results.witnesses[0];
+		wWitnessOnB = results.witnesses[1];
+		return true;		
+		}
+
+	return false;
+}
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h	(revision 1963)
@@ -0,0 +1,39 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+EPA Copyright (c) Ricardo Padrela 2006 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
+#define BT_GJP_EPA_PENETRATION_DEPTH_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+///calculate the penetration depth between two convex shapes.
+class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+	public :
+
+		bool			calcPenDepth( btSimplexSolverInterface& simplexSolver,
+									  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
+									  const btTransform& transformA, const btTransform& transformB,
+									  btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
+									  class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
+
+	private :
+
+};
+
+#endif	// BT_GJP_EPA_PENETRATION_DEPTH_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp	(revision 1963)
@@ -0,0 +1,351 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+//#define TEST_NON_VIRTUAL 1
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+//#define DEBUG_SPU_COLLISION_DETECTION 1
+#endif //__SPU__
+#endif
+
+//must be above the machine epsilon
+#define REL_ERROR2 btScalar(1.0e-6)
+
+//temp globals, to improve GJK/EPA/penetration calculations
+int gNumDeepPenetrationChecks = 0;
+int gNumGjkChecks = 0;
+
+
+
+btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(1)
+{
+}
+
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+{
+	btScalar distance=btScalar(0.);
+	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 pointOnA,pointOnB;
+	btTransform	localTransA = input.m_transformA;
+	btTransform localTransB = input.m_transformB;
+	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+	localTransA.getOrigin() -= positionOffset;
+	localTransB.getOrigin() -= positionOffset;
+
+
+	btScalar marginA = m_minkowskiA->getMarginNonVirtual();
+	btScalar marginB = m_minkowskiB->getMarginNonVirtual();
+
+#ifdef TEST_NON_VIRTUAL
+	btScalar marginAv = m_minkowskiA->getMargin();
+	btScalar marginBv = m_minkowskiB->getMargin();
+
+	btAssert(marginA == marginAv);
+	btAssert(marginB == marginBv);
+#endif //TEST_NON_VIRTUAL
+
+	gNumGjkChecks++;
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+	spu_printf("inside gjk\n");
+#endif
+	//for CCD we don't use margins
+	if (m_ignoreMargin)
+	{
+		marginA = btScalar(0.);
+		marginB = btScalar(0.);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("ignoring margin\n");
+#endif
+	}
+
+	m_curIter = 0;
+	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0,1,0);
+
+	bool isValid = false;
+	bool checkSimplex = false;
+	bool checkPenetration = true;
+	m_degenerateSimplex = 0;
+
+	m_lastUsedMethod = -1;
+
+	{
+		btScalar squaredDistance = SIMD_INFINITY;
+		btScalar delta = btScalar(0.);
+		
+		btScalar margin = marginA + marginB;
+		
+		
+
+		m_simplexSolver->reset();
+		
+		for ( ; ; )
+		//while (true)
+		{
+
+			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+#ifdef TEST_NON_VIRTUAL
+			btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+			btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+#endif 
+			btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+			btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+#ifdef TEST_NON_VIRTUAL
+			btAssert((pInAv-pInA).length() < 0.0001);
+			btAssert((qInBv-qInB).length() < 0.0001);
+#endif //
+
+			btPoint3  pWorld = localTransA(pInA);	
+			btPoint3  qWorld = localTransB(qInB);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("got local supporting vertices\n");
+#endif
+
+			btVector3 w	= pWorld - qWorld;
+			delta = m_cachedSeparatingAxis.dot(w);
+
+			// potential exit, they don't overlap
+			if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) 
+			{
+				checkPenetration = false;
+				break;
+			}
+
+			//exit 0: the new point is already in the simplex, or we didn't come any closer
+			if (m_simplexSolver->inSimplex(w))
+			{
+				m_degenerateSimplex = 1;
+				checkSimplex = true;
+				break;
+			}
+			// are we getting any closer ?
+			btScalar f0 = squaredDistance - delta;
+			btScalar f1 = squaredDistance * REL_ERROR2;
+
+			if (f0 <= f1)
+			{
+				if (f0 <= btScalar(0.))
+				{
+					m_degenerateSimplex = 2;
+				}
+				checkSimplex = true;
+				break;
+			}
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("addVertex 1\n");
+#endif
+			//add current vertex to simplex
+			m_simplexSolver->addVertex(w, pWorld, qWorld);
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("addVertex 2\n");
+#endif
+			//calculate the closest point to the origin (update vector v)
+			if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+			{
+				m_degenerateSimplex = 3;
+				checkSimplex = true;
+				break;
+			}
+
+			if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
+            {
+                m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+
+			btScalar previousSquaredDistance = squaredDistance;
+			squaredDistance = m_cachedSeparatingAxis.length2();
+			
+			//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+			//are we getting any closer ?
+			if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) 
+			{ 
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				checkSimplex = true;
+				break;
+			}
+
+			  //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+              if (m_curIter++ > gGjkMaxIter)   
+              {   
+                      #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)
+
+                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
+                              printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
+                              m_cachedSeparatingAxis.getX(),   
+                              m_cachedSeparatingAxis.getY(),   
+                              m_cachedSeparatingAxis.getZ(),   
+                              squaredDistance,   
+                              m_minkowskiA->getShapeType(),   
+                              m_minkowskiB->getShapeType());   
+
+                      #endif   
+                      break;   
+
+              } 
+
+
+			bool check = (!m_simplexSolver->fullSimplex());
+			//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+			if (!check)
+			{
+				//do we need this backup_closest here ?
+				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+				break;
+			}
+		}
+
+		if (checkSimplex)
+		{
+			m_simplexSolver->compute_points(pointOnA, pointOnB);
+			normalInB = pointOnA-pointOnB;
+			btScalar lenSqr = m_cachedSeparatingAxis.length2();
+			//valid normal
+			if (lenSqr < 0.0001)
+			{
+				m_degenerateSimplex = 5;
+			} 
+			if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+			{
+				btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+				normalInB *= rlen; //normalize
+				btScalar s = btSqrt(squaredDistance);
+			
+				btAssert(s > btScalar(0.0));
+				pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+				pointOnB += m_cachedSeparatingAxis * (marginB / s);
+				distance = ((btScalar(1.)/rlen) - margin);
+				isValid = true;
+				
+				m_lastUsedMethod = 1;
+			} else
+			{
+				m_lastUsedMethod = 2;
+			}
+		}
+
+		bool catchDegeneratePenetrationCase = 
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+
+		//if (checkPenetration && !isValid)
+		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		{
+			//penetration case
+		
+			//if there is no way to handle penetrations, bail out
+			if (m_penetrationDepthSolver)
+			{
+				// Penetration depth case.
+				btVector3 tmpPointOnA,tmpPointOnB;
+				
+				gNumDeepPenetrationChecks++;
+
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
+					*m_simplexSolver, 
+					m_minkowskiA,m_minkowskiB,
+					localTransA,localTransB,
+					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+					debugDraw,input.m_stackAlloc
+					);
+
+				if (isValid2)
+				{
+					btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+					btScalar lenSqr = tmpNormalInB.length2();
+					if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+					{
+						tmpNormalInB /= btSqrt(lenSqr);
+						btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+						//only replace valid penetrations when the result is deeper (check)
+						if (!isValid || (distance2 < distance))
+						{
+							distance = distance2;
+							pointOnA = tmpPointOnA;
+							pointOnB = tmpPointOnB;
+							normalInB = tmpNormalInB;
+							isValid = true;
+							m_lastUsedMethod = 3;
+						} else
+						{
+							
+						}
+					} else
+					{
+						//isValid = false;
+						m_lastUsedMethod = 4;
+					}
+				} else
+				{
+					m_lastUsedMethod = 5;
+				}
+				
+			}
+		}
+	}
+
+	if (isValid)
+	{
+#ifdef __SPU__
+		//spu_printf("distance\n");
+#endif //__CELLOS_LV2__
+
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("output 1\n");
+#endif
+		output.addContactPoint(
+			normalInB,
+			pointOnB+positionOffset,
+			distance);
+
+#ifdef DEBUG_SPU_COLLISION_DETECTION
+		spu_printf("output 2\n");
+#endif
+		//printf("gjk add:%f",distance);
+	}
+
+
+}
+
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h	(revision 1963)
@@ -0,0 +1,85 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+
+#ifndef GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/btPoint3.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+
+class btConvexShape;
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
+{
+	
+
+	btVector3	m_cachedSeparatingAxis;
+	btConvexPenetrationDepthSolver*	m_penetrationDepthSolver;
+	btSimplexSolverInterface* m_simplexSolver;
+	const btConvexShape* m_minkowskiA;
+	const btConvexShape* m_minkowskiB;
+	bool		m_ignoreMargin;
+	
+
+public:
+
+	//some debugging to fix degeneracy problems
+	int			m_lastUsedMethod;
+	int			m_curIter;
+	int			m_degenerateSimplex;
+	int			m_catchDegeneracies;
+
+
+	btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver);
+	virtual ~btGjkPairDetector() {};
+
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+	void setMinkowskiA(btConvexShape* minkA)
+	{
+		m_minkowskiA = minkA;
+	}
+
+	void setMinkowskiB(btConvexShape* minkB)
+	{
+		m_minkowskiB = minkB;
+	}
+	void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+	{
+		m_cachedSeparatingAxis = seperatingAxis;
+	}
+
+	void	setPenetrationDepthSolver(btConvexPenetrationDepthSolver*	penetrationDepthSolver)
+	{
+		m_penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	///don't use setIgnoreMargin, it's for Bullet's internal use
+	void	setIgnoreMargin(bool ignoreMargin)
+	{
+		m_ignoreMargin = ignoreMargin;
+	}
+
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h	(revision 1963)
@@ -0,0 +1,117 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MANIFOLD_CONTACT_POINT_H
+#define MANIFOLD_CONTACT_POINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+
+
+/// ManifoldContactPoint collects and maintains persistent contactpoints.
+/// used to improve stability and performance of rigidbody dynamics response.
+class btManifoldPoint
+	{
+		public:
+			btManifoldPoint()
+				:m_userPersistentData(0),
+				m_appliedImpulse(0.f),
+				m_lateralFrictionInitialized(false),
+				m_lifeTime(0)
+			{
+			}
+
+			btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, 
+					const btVector3 &normal, 
+					btScalar distance ) :
+					m_localPointA( pointA ), 
+					m_localPointB( pointB ), 
+					m_normalWorldOnB( normal ), 
+					m_distance1( distance ),
+					m_combinedFriction(btScalar(0.)),
+					m_combinedRestitution(btScalar(0.)),
+					m_userPersistentData(0),
+					m_appliedImpulse(0.f),
+					m_lateralFrictionInitialized(false),
+					m_appliedImpulseLateral1(0.f),
+					m_appliedImpulseLateral2(0.f),
+					m_lifeTime(0)
+			{
+				
+					
+			}
+
+			
+
+			btVector3 m_localPointA;			
+			btVector3 m_localPointB;			
+			btVector3	m_positionWorldOnB;
+			///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+			btVector3	m_positionWorldOnA;
+			btVector3 m_normalWorldOnB;
+		
+			btScalar	m_distance1;
+			btScalar	m_combinedFriction;
+			btScalar	m_combinedRestitution;
+
+         //BP mod, store contact triangles.
+         int	   m_partId0;
+         int      m_partId1;
+         int      m_index0;
+         int      m_index1;
+				
+			mutable void*	m_userPersistentData;
+			btScalar		m_appliedImpulse;
+
+			bool			m_lateralFrictionInitialized;
+			btScalar		m_appliedImpulseLateral1;
+			btScalar		m_appliedImpulseLateral2;
+			int				m_lifeTime;//lifetime of the contactpoint in frames
+			
+			btVector3		m_lateralFrictionDir1;
+			btVector3		m_lateralFrictionDir2;
+
+			btScalar getDistance() const
+			{
+				return m_distance1;
+			}
+			int	getLifeTime() const
+			{
+				return m_lifeTime;
+			}
+
+			const btVector3& getPositionWorldOnA() const {
+				return m_positionWorldOnA;
+//				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+			}
+
+			const btVector3& getPositionWorldOnB() const
+			{
+				return m_positionWorldOnB;
+			}
+
+			void	setDistance(btScalar dist)
+			{
+				m_distance1 = dist;
+			}
+			
+			
+
+	};
+
+#endif //MANIFOLD_CONTACT_POINT_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp	(revision 1963)
@@ -0,0 +1,334 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#define NUM_UNITSPHERE_POINTS 42
+static btVector3	sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
+{
+btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
+btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
+btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
+btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
+btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
+btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
+btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
+btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
+btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
+btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
+btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
+btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
+btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
+btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
+btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
+btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
+btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
+btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
+btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
+btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
+btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
+btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
+btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
+btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
+btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
+btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
+btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
+btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
+btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
+btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
+btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
+btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
+btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
+};
+
+
+bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+												   const btConvexShape* convexA,const btConvexShape* convexB,
+												   const btTransform& transA,const btTransform& transB,
+												   btVector3& v, btPoint3& pa, btPoint3& pb,
+												   class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+												   )
+{
+
+	(void)stackAlloc;
+	(void)v;
+	
+
+	struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
+	{
+
+		btIntermediateResult():m_hasResult(false)
+		{
+		}
+		
+		btVector3 m_normalOnBInWorld;
+		btVector3 m_pointInWorld;
+		btScalar m_depth;
+		bool	m_hasResult;
+
+		virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+		{
+			(void)partId0;
+			(void)index0;
+			(void)partId1;
+			(void)index1;
+		}
+		void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+		{
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			m_depth = depth;
+			m_hasResult = true;
+		}
+	};
+
+	//just take fixed number of orientation, and sample the penetration depth in that direction
+	btScalar minProj = btScalar(1e30);
+	btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 minA,minB;
+	btVector3 seperatingAxisInA,seperatingAxisInB;
+	btVector3 pInA,qInB,pWorld,qWorld,w;
+
+#ifndef __SPU__
+#define USE_BATCHED_SUPPORT 1
+#endif
+#ifdef USE_BATCHED_SUPPORT
+
+	btVector3	supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	btVector3	seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+	int i;
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
+		seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
+	}
+
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+				seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+				numSampleDirections++;
+			}
+		}
+	}
+
+
+
+	convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+	convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+	for (i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = seperatingAxisInABatch[i];
+		seperatingAxisInB = seperatingAxisInBBatch[i];
+
+		pInA = supportVerticesABatch[i];
+		qInB = supportVerticesBBatch[i];
+
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}	
+#else
+
+	int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+#ifndef __SPU__
+	{
+		int numPDA = convexA->getNumPreferredPenetrationDirections();
+		if (numPDA)
+		{
+			for (int i=0;i<numPDA;i++)
+			{
+				btVector3 norm;
+				convexA->getPreferredPenetrationDirection(i,norm);
+				norm  = transA.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+
+	{
+		int numPDB = convexB->getNumPreferredPenetrationDirections();
+		if (numPDB)
+		{
+			for (int i=0;i<numPDB;i++)
+			{
+				btVector3 norm;
+				convexB->getPreferredPenetrationDirection(i,norm);
+				norm  = transB.getBasis() * norm;
+				sPenetrationDirections[numSampleDirections] = norm;
+				numSampleDirections++;
+			}
+		}
+	}
+#endif // __SPU__
+
+	for (int i=0;i<numSampleDirections;i++)
+	{
+		const btVector3& norm = sPenetrationDirections[i];
+		seperatingAxisInA = (-norm)* transA.getBasis();
+		seperatingAxisInB = norm* transB.getBasis();
+		pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+		qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+		pWorld = transA(pInA);	
+		qWorld = transB(qInB);
+		w	= qWorld - pWorld;
+		btScalar delta = norm.dot(w);
+		//find smallest delta
+		if (delta < minProj)
+		{
+			minProj = delta;
+			minNorm = norm;
+			minA = pWorld;
+			minB = qWorld;
+		}
+	}
+#endif //USE_BATCHED_SUPPORT
+
+	//add the margins
+
+	minA += minNorm*convexA->getMarginNonVirtual();
+	minB -= minNorm*convexB->getMarginNonVirtual();
+	//no penetration
+	if (minProj < btScalar(0.))
+		return false;
+
+	minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
+
+
+
+
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+	if (debugDraw)
+	{
+		btVector3 color(0,1,0);
+		debugDraw->drawLine(minA,minB,color);
+		color = btVector3 (1,1,1);
+		btVector3 vec = minB-minA;
+		btScalar prj2 = minNorm.dot(vec);
+		debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
+
+	}
+#endif //DEBUG_DRAW
+
+	
+
+	btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+	btScalar offsetDist = minProj;
+	btVector3 offset = minNorm * offsetDist;
+	
+
+
+	btGjkPairDetector::ClosestPointInput input;
+		
+	btVector3 newOrg = transA.getOrigin() + offset;
+
+	btTransform displacedTrans = transA;
+	displacedTrans.setOrigin(newOrg);
+
+	input.m_transformA = displacedTrans;
+	input.m_transformB = transB;
+	input.m_maximumDistanceSquared = btScalar(1e30);//minProj;
+	
+	btIntermediateResult res;
+	gjkdet.getClosestPoints(input,res,debugDraw);
+
+	btScalar correctedMinNorm = minProj - res.m_depth;
+
+
+	//the penetration depth is over-estimated, relax it
+	btScalar penetration_relaxation= btScalar(1.);
+	minNorm*=penetration_relaxation;
+
+	if (res.m_hasResult)
+	{
+
+		pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+		pb = res.m_pointInWorld;
+		
+#ifdef DEBUG_DRAW
+		if (debugDraw)
+		{
+			btVector3 color(1,0,0);
+			debugDraw->drawLine(pa,pb,color);
+		}
+#endif//DEBUG_DRAW
+
+
+	}
+	return res.m_hasResult;
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h	(revision 1963)
@@ -0,0 +1,36 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+public:
+
+	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+	const btConvexShape* convexA,const btConvexShape* convexB,
+				const btTransform& transA,const btTransform& transB,
+			btVector3& v, btPoint3& pa, btPoint3& pb,
+			class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+			);
+};
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp	(revision 1963)
@@ -0,0 +1,256 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btPersistentManifold.h"
+#include "LinearMath/btTransform.h"
+#include <assert.h>
+
+btScalar					gContactBreakingThreshold = btScalar(0.02);
+ContactDestroyedCallback	gContactDestroyedCallback = 0;
+ContactProcessedCallback	gContactProcessedCallback = 0;
+
+
+
+btPersistentManifold::btPersistentManifold()
+:m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+m_index1a(0)
+{
+}
+
+
+
+
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void	btPersistentManifold::DebugPersistency()
+{
+	int i;
+	printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
+	for (i=0;i<m_cachedPoints;i++)
+	{
+		printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+	}
+}
+#endif //DEBUG_PERSISTENCY
+
+void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
+{
+
+	void* oldPtr = pt.m_userPersistentData;
+	if (oldPtr)
+	{
+#ifdef DEBUG_PERSISTENCY
+		int i;
+		int occurance = 0;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			if (m_pointCache[i].m_userPersistentData == oldPtr)
+			{
+				occurance++;
+				if (occurance>1)
+					printf("error in clearUserCache\n");
+			}
+		}
+		assert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+		if (pt.m_userPersistentData && gContactDestroyedCallback)
+		{
+			(*gContactDestroyedCallback)(pt.m_userPersistentData);
+			pt.m_userPersistentData = 0;
+		}
+		
+#ifdef DEBUG_PERSISTENCY
+		DebugPersistency();
+#endif
+	}
+
+	
+}
+
+
+int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
+{
+
+		//calculate 4 possible cases areas, and take biggest area
+		//also need to keep 'deepest'
+		
+		int maxPenetrationIndex = -1;
+#define KEEP_DEEPEST_POINT 1
+#ifdef KEEP_DEEPEST_POINT
+		btScalar maxPenetration = pt.getDistance();
+		for (int i=0;i<4;i++)
+		{
+			if (m_pointCache[i].getDistance() < maxPenetration)
+			{
+				maxPenetrationIndex = i;
+				maxPenetration = m_pointCache[i].getDistance();
+			}
+		}
+#endif //KEEP_DEEPEST_POINT
+		
+		btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
+		if (maxPenetrationIndex != 0)
+		{
+			btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 cross = a0.cross(b0);
+			res0 = cross.length2();
+		}
+		if (maxPenetrationIndex != 1)
+		{
+			btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+			btVector3 cross = a1.cross(b1);
+			res1 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 2)
+		{
+			btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 cross = a2.cross(b2);
+			res2 = cross.length2();
+		}
+
+		if (maxPenetrationIndex != 3)
+		{
+			btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+			btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+			btVector3 cross = a3.cross(b3);
+			res3 = cross.length2();
+		}
+
+		btVector4 maxvec(res0,res1,res2,res3);
+		int biggestarea = maxvec.closestAxis4();
+		return biggestarea;
+}
+
+
+int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
+{
+	btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
+	int size = getNumContacts();
+	int nearestPoint = -1;
+	for( int i = 0; i < size; i++ )
+	{
+		const btManifoldPoint &mp = m_pointCache[i];
+
+		btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
+		const btScalar distToManiPoint = diffA.dot(diffA);
+		if( distToManiPoint < shortestDist )
+		{
+			shortestDist = distToManiPoint;
+			nearestPoint = i;
+		}
+	}
+	return nearestPoint;
+}
+
+int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
+{
+	assert(validContactDistance(newPoint));
+
+	int insertIndex = getNumContacts();
+	if (insertIndex == MANIFOLD_CACHE_SIZE)
+	{
+#if MANIFOLD_CACHE_SIZE >= 4
+		//sort cache so best points come first, based on area
+		insertIndex = sortCachedPoints(newPoint);
+#else
+		insertIndex = 0;
+#endif
+		clearUserCache(m_pointCache[insertIndex]);
+		
+	} else
+	{
+		m_cachedPoints++;
+
+		
+	}
+	btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
+	m_pointCache[insertIndex] = newPoint;
+	return insertIndex;
+}
+
+btScalar	btPersistentManifold::getContactBreakingThreshold() const
+{
+	return gContactBreakingThreshold;
+}
+
+
+
+void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
+{
+	int i;
+#ifdef DEBUG_PERSISTENCY
+	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+		trA.getOrigin().getX(),
+		trA.getOrigin().getY(),
+		trA.getOrigin().getZ(),
+		trB.getOrigin().getX(),
+		trB.getOrigin().getY(),
+		trB.getOrigin().getZ());
+#endif //DEBUG_PERSISTENCY
+	/// first refresh worldspace positions and distance
+	for (i=getNumContacts()-1;i>=0;i--)
+	{
+		btManifoldPoint &manifoldPoint = m_pointCache[i];
+		manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
+		manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
+		manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+		manifoldPoint.m_lifeTime++;
+	}
+
+	/// then 
+	btScalar distance2d;
+	btVector3 projectedDifference,projectedPoint;
+	for (i=getNumContacts()-1;i>=0;i--)
+	{
+		
+		btManifoldPoint &manifoldPoint = m_pointCache[i];
+		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+		if (!validContactDistance(manifoldPoint))
+		{
+			removeContactPoint(i);
+		} else
+		{
+			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+			projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
+			projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
+			distance2d = projectedDifference.dot(projectedDifference);
+			if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
+			{
+				removeContactPoint(i);
+			} else
+			{
+				//contact point processed callback
+				if (gContactProcessedCallback)
+					(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
+			}
+		}
+	}
+#ifdef DEBUG_PERSISTENCY
+	DebugPersistency();
+#endif //
+}
+
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h	(revision 1963)
@@ -0,0 +1,193 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btManifoldPoint.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+struct btCollisionResult;
+
+///contact breaking and merging threshold
+extern btScalar gContactBreakingThreshold;
+
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
+extern ContactDestroyedCallback	gContactDestroyedCallback;
+
+
+
+
+#define MANIFOLD_CACHE_SIZE 4
+
+///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
+///Those contact points are created by the collision narrow phase.
+///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
+///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
+///reduces the cache to 4 points, when more then 4 points are added, using following rules:
+///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
+///note that some pairs of objects might have more then one contact manifold.
+ATTRIBUTE_ALIGNED16( class) btPersistentManifold 
+{
+
+	btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
+
+	/// this two body pointers can point to the physics rigidbody class.
+	/// void* will allow any rigidbody class
+	void* m_body0;
+	void* m_body1;
+	int	m_cachedPoints;
+
+	
+	/// sort cached points so most isolated points come first
+	int	sortCachedPoints(const btManifoldPoint& pt);
+
+	int		findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_index1a;
+
+	btPersistentManifold();
+
+	btPersistentManifold(void* body0,void* body1,int bla)
+		: m_body0(body0),m_body1(body1),m_cachedPoints(0)
+	{
+		(void)bla;
+	}
+
+	SIMD_FORCE_INLINE void* getBody0() { return m_body0;}
+	SIMD_FORCE_INLINE void* getBody1() { return m_body1;}
+
+	SIMD_FORCE_INLINE const void* getBody0() const { return m_body0;}
+	SIMD_FORCE_INLINE const void* getBody1() const { return m_body1;}
+
+	void	setBodies(void* body0,void* body1)
+	{
+		m_body0 = body0;
+		m_body1 = body1;
+	}
+
+	void clearUserCache(btManifoldPoint& pt);
+
+#ifdef DEBUG_PERSISTENCY
+	void	DebugPersistency();
+#endif //
+	
+	SIMD_FORCE_INLINE int	getNumContacts() const { return m_cachedPoints;}
+
+	SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
+	{
+		btAssert(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
+	{
+		btAssert(index < m_cachedPoints);
+		return m_pointCache[index];
+	}
+
+	/// todo: get this margin from the current physics / collision environment
+	btScalar	getContactBreakingThreshold() const;
+	
+	int getCacheEntry(const btManifoldPoint& newPoint) const;
+
+	int addManifoldPoint( const btManifoldPoint& newPoint);
+
+	void removeContactPoint (int index)
+	{
+		clearUserCache(m_pointCache[index]);
+
+		int lastUsedIndex = getNumContacts() - 1;
+//		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		if(index != lastUsedIndex) 
+		{
+			m_pointCache[index] = m_pointCache[lastUsedIndex]; 
+			//get rid of duplicated userPersistentData pointer
+			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
+			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
+			m_pointCache[lastUsedIndex].m_lifeTime = 0;
+		}
+
+		btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
+		m_cachedPoints--;
+	}
+	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+	{
+		btAssert(validContactDistance(newPoint));
+
+#define MAINTAIN_PERSISTENCY 1
+#ifdef MAINTAIN_PERSISTENCY
+		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btScalar	appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
+		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
+				
+		btAssert(lifeTime>=0);
+		void* cache = m_pointCache[insertIndex].m_userPersistentData;
+		
+		m_pointCache[insertIndex] = newPoint;
+
+		m_pointCache[insertIndex].m_userPersistentData = cache;
+		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
+		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
+		
+		m_pointCache[insertIndex].m_lifeTime = lifeTime;
+#else
+		clearUserCache(m_pointCache[insertIndex]);
+		m_pointCache[insertIndex] = newPoint;
+	
+#endif
+	}
+
+	bool validContactDistance(const btManifoldPoint& pt) const
+	{
+		return pt.m_distance1 <= getContactBreakingThreshold();
+	}
+	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+	void	refreshContactPoints(  const btTransform& trA,const btTransform& trB);
+
+	
+	SIMD_FORCE_INLINE	void	clearManifold()
+	{
+		int i;
+		for (i=0;i<m_cachedPoints;i++)
+		{
+			clearUserCache(m_pointCache[i]);
+		}
+		m_cachedPoints = 0;
+	}
+
+
+
+}
+;
+
+
+
+
+
+#endif //PERSISTENT_MANIFOLD_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h	(revision 1963)
@@ -0,0 +1,61 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POINT_COLLECTOR_H
+#define POINT_COLLECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+
+
+
+struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
+{
+	
+	
+	btVector3 m_normalOnBInWorld;
+	btVector3 m_pointInWorld;
+	btScalar	m_distance;//negative means penetration
+
+	bool	m_hasResult;
+
+	btPointCollector () 
+		: m_distance(btScalar(1e30)),m_hasResult(false)
+	{
+	}
+
+	virtual void setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
+	{
+		(void)partId0;
+		(void)index0;
+		(void)partId1;
+		(void)index1;
+		//??
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	{
+		if (depth< m_distance)
+		{
+			m_hasResult = true;
+			m_normalOnBInWorld = normalOnBInWorld;
+			m_pointInWorld = pointInWorld;
+			//negative means penetration
+			m_distance = depth;
+		}
+	}
+};
+
+#endif //POINT_COLLECTOR_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp	(revision 1963)
@@ -0,0 +1,163 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//#include <stdio.h>
+
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "btRaycastCallback.h"
+
+btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
+	:
+	m_from(from),
+	m_to(to),
+	m_hitFraction(btScalar(1.))
+{
+
+}
+
+
+
+void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+	const btVector3 &vert0=triangle[0];
+	const btVector3 &vert1=triangle[1];
+	const btVector3 &vert2=triangle[2];
+
+	btVector3 v10; v10 = vert1 - vert0 ;
+	btVector3 v20; v20 = vert2 - vert0 ;
+
+	btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+	
+	const btScalar dist = vert0.dot(triangleNormal);
+	btScalar dist_a = triangleNormal.dot(m_from) ;
+	dist_a-= dist;
+	btScalar dist_b = triangleNormal.dot(m_to);
+	dist_b -= dist;
+
+	if ( dist_a * dist_b >= btScalar(0.0) )
+	{
+		return ; // same sign
+	}
+	
+	const btScalar proj_length=dist_a-dist_b;
+	const btScalar distance = (dist_a)/(proj_length);
+	// Now we have the intersection point on the plane, we'll see if it's inside the triangle
+	// Add an epsilon as a tolerance for the raycast,
+	// in case the ray hits exacly on the edge of the triangle.
+	// It must be scaled for the triangle size.
+	
+	if(distance < m_hitFraction)
+	{
+		
+
+		btScalar edge_tolerance =triangleNormal.length2();		
+		edge_tolerance *= btScalar(-0.0001);
+		btVector3 point; point.setInterpolate3( m_from, m_to, distance);
+		{
+			btVector3 v0p; v0p = vert0 - point;
+			btVector3 v1p; v1p = vert1 - point;
+			btVector3 cp0; cp0 = v0p.cross( v1p );
+
+			if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) 
+			{
+						
+
+				btVector3 v2p; v2p = vert2 -  point;
+				btVector3 cp1;
+				cp1 = v1p.cross( v2p);
+				if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) 
+				{
+					btVector3 cp2;
+					cp2 = v2p.cross(v0p);
+					
+					if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) 
+					{
+
+						if ( dist_a > 0 )
+						{
+							m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+						}
+						else
+						{
+							m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+
+btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
+{
+	m_convexShape = convexShape;
+	m_convexShapeFrom = convexShapeFrom;
+	m_convexShapeTo = convexShapeTo;
+	m_triangleToWorld = triangleToWorld;
+	m_hitFraction = 1.0;
+    m_triangleCollisionMargin = triangleCollisionMargin;
+}
+
+void
+btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
+{
+	btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
+    triangleShape.setMargin(m_triangleCollisionMargin);
+
+	btVoronoiSimplexSolver	simplexSolver;
+	btGjkEpaPenetrationDepthSolver	gjkEpaPenetrationSolver;
+
+//#define  USE_SUBSIMPLEX_CONVEX_CAST 1
+//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+	btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
+#else
+	//btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
+	btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
+#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+	
+	btConvexCast::CastResult castResult;
+	castResult.m_fraction = btScalar(1.);
+	if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
+	{
+		//add hit
+		if (castResult.m_normal.length2() > btScalar(0.0001))
+		{					
+			if (castResult.m_fraction < m_hitFraction)
+			{
+/* btContinuousConvexCast's normal is already in world space */
+/*
+#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+				//rotate normal into worldspace
+				castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
+#endif //USE_SUBSIMPLEX_CONVEX_CAST
+*/
+				castResult.m_normal.normalize();
+
+				reportHit (castResult.m_normal,
+							castResult.m_hitPoint,
+							castResult.m_fraction,
+							partId,
+							triangleIndex);
+			}
+		}
+	}
+}
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h	(revision 1963)
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "LinearMath/btTransform.h"
+struct btBroadphaseProxy;
+class btConvexShape;
+
+class  btTriangleRaycastCallback: public btTriangleCallback
+{
+public:
+
+	//input
+	btVector3 m_from;
+	btVector3 m_to;
+
+	btScalar	m_hitFraction;
+
+	btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
+	
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+	virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0;
+	
+};
+
+class btTriangleConvexcastCallback : public btTriangleCallback
+{
+public:
+	const btConvexShape* m_convexShape;
+	btTransform m_convexShapeFrom;
+	btTransform m_convexShapeTo;
+	btTransform m_triangleToWorld;
+	btScalar m_hitFraction;
+    btScalar m_triangleCollisionMargin;
+
+	btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
+
+	virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
+
+	virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h	(revision 1963)
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btPoint3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "btVoronoiSimplexSolver.h"
+#define btSimplexSolverInterface btVoronoiSimplexSolver
+#else
+
+/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+/// voronoi regions or barycentric coordinates
+class btSimplexSolverInterface
+{
+	public:
+		virtual ~btSimplexSolverInterface() {};
+
+	virtual void reset() = 0;
+
+	virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0;
+	
+	virtual bool closest(btVector3& v) = 0;
+
+	virtual btScalar maxVertex() = 0;
+
+	virtual bool fullSimplex() const = 0;
+
+	virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0;
+
+	virtual bool inSimplex(const btVector3& w) = 0;
+	
+	virtual void backup_closest(btVector3& v) = 0;
+
+	virtual bool emptySimplex() const = 0;
+
+	virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0;
+
+	virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp	(revision 1963)
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSubSimplexConvexCast.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "btPointCollector.h"
+#include "LinearMath/btTransformUtil.h"
+
+btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#else
+#define MAX_ITERATIONS 32
+#endif
+bool	btSubsimplexConvexCast::calcTimeOfImpact(
+		const btTransform& fromA,
+		const btTransform& toA,
+		const btTransform& fromB,
+		const btTransform& toB,
+		CastResult& result)
+{
+
+	m_simplexSolver->reset();
+
+	btVector3 linVelA,linVelB;
+	linVelA = toA.getOrigin()-fromA.getOrigin();
+	linVelB = toB.getOrigin()-fromB.getOrigin();
+
+	btScalar lambda = btScalar(0.);
+
+	btTransform interpolatedTransA = fromA;
+	btTransform interpolatedTransB = fromB;
+
+	///take relative motion
+	btVector3 r = (linVelA-linVelB);
+	btVector3 v;
+	
+	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
+	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
+	v = supVertexA-supVertexB;
+	int maxIter = MAX_ITERATIONS;
+
+	btVector3 n;
+	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	bool hasResult = false;
+	btVector3 c;
+
+	btScalar lastLambda = lambda;
+
+
+	btScalar dist2 = v.length2();
+#ifdef BT_USE_DOUBLE_PRECISION
+	btScalar epsilon = btScalar(0.0001);
+#else
+	btScalar epsilon = btScalar(0.0001);
+#endif //BT_USE_DOUBLE_PRECISION
+	btVector3	w,p;
+	btScalar VdotR;
+	
+	while ( (dist2 > epsilon) && maxIter--)
+	{
+		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
+		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
+		w = supVertexA-supVertexB;
+
+		btScalar VdotW = v.dot(w);
+
+		if (lambda > btScalar(1.0))
+		{
+			return false;
+		}
+
+		if ( VdotW > btScalar(0.))
+		{
+			VdotR = v.dot(r);
+
+			if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+				return false;
+			else
+			{
+				lambda = lambda - VdotW / VdotR;
+				//interpolate to next lambda
+				//	x = s + lambda * r;
+				interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
+				interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
+				//m_simplexSolver->reset();
+				//check next line
+				 w = supVertexA-supVertexB;
+				lastLambda = lambda;
+				n = v;
+				hasResult = true;
+			}
+		} 
+		m_simplexSolver->addVertex( w, supVertexA , supVertexB);
+		if (m_simplexSolver->closest(v))
+		{
+			dist2 = v.length2();
+			hasResult = true;
+			//todo: check this normal for validity
+			//n=v;
+			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+			//printf("DIST2=%f\n",dist2);
+			//printf("numverts = %i\n",m_simplexSolver->numVertices());
+		} else
+		{
+			dist2 = btScalar(0.);
+		} 
+	}
+
+	//int numiter = MAX_ITERATIONS - maxIter;
+//	printf("number of iterations: %d", numiter);
+	
+	//don't report a time of impact when moving 'away' from the hitnormal
+	
+
+	result.m_fraction = lambda;
+	if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
+		result.m_normal = n.normalized();
+	else
+		result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+
+	//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+	if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
+		return false;
+
+	btVector3 hitA,hitB;
+	m_simplexSolver->compute_points(hitA,hitB);
+	result.m_hitPoint=hitB;
+	return true;
+}
+
+
+
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h	(revision 1963)
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SUBSIMPLEX_CONVEX_CAST_H
+#define SUBSIMPLEX_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+
+/// btSubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class btSubsimplexConvexCast : public btConvexCast
+{
+	btSimplexSolverInterface* m_simplexSolver;
+	const btConvexShape*	m_convexA;
+	const btConvexShape*	m_convexB;
+
+public:
+
+	btSubsimplexConvexCast (const btConvexShape*	shapeA,const btConvexShape*	shapeB,btSimplexSolverInterface* simplexSolver);
+
+	//virtual ~btSubsimplexConvexCast();
+	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
+	virtual bool	calcTimeOfImpact(
+			const btTransform& fromA,
+			const btTransform& toA,
+			const btTransform& fromB,
+			const btTransform& toB,
+			CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp	(revision 1963)
@@ -0,0 +1,607 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+	
+	Elsevier CDROM license agreements grants nonexclusive license to use the software
+	for any purpose, commercial or non-commercial as long as the following credit is included
+	identifying the original source of the software:
+
+	Parts of the source are "from the book Real-Time Collision Detection by
+	Christer Ericson, published by Morgan Kaufmann Publishers,
+	(c) 2005 Elsevier Inc."
+		
+*/
+
+
+#include "btVoronoiSimplexSolver.h"
+#include <assert.h>
+//#include <stdio.h>
+
+#define VERTA  0
+#define VERTB  1
+#define VERTC  2
+#define VERTD  3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void	btVoronoiSimplexSolver::removeVertex(int index)
+{
+	
+	assert(m_numVertices>0);
+	m_numVertices--;
+	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void	btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
+{
+	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+		removeVertex(3);
+
+	if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+		removeVertex(2);
+
+	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+		removeVertex(1);
+	
+	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+		removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void btVoronoiSimplexSolver::reset()
+{
+	m_cachedValidClosest = false;
+	m_numVertices = 0;
+	m_needsUpdate = true;
+	m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	m_cachedBC.reset();
+}
+
+
+
+	//add a vertex
+void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
+{
+	m_lastW = w;
+	m_needsUpdate = true;
+
+	m_simplexVectorW[m_numVertices] = w;
+	m_simplexPointsP[m_numVertices] = p;
+	m_simplexPointsQ[m_numVertices] = q;
+
+	m_numVertices++;
+}
+
+bool	btVoronoiSimplexSolver::updateClosestVectorAndPoints()
+{
+	
+	if (m_needsUpdate)
+	{
+		m_cachedBC.reset();
+
+		m_needsUpdate = false;
+
+		switch (numVertices())
+		{
+		case 0:
+				m_cachedValidClosest = false;
+				break;
+		case 1:
+			{
+				m_cachedP1 = m_simplexPointsP[0];
+				m_cachedP2 = m_simplexPointsQ[0];
+				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedBC.reset();
+				m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
+			};
+		case 2:
+			{
+			//closest point origin from line segment
+					const btVector3& from = m_simplexVectorW[0];
+					const btVector3& to = m_simplexVectorW[1];
+					btVector3 nearest;
+
+					btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+					btVector3 diff = p - from;
+					btVector3 v = to - from;
+					btScalar t = v.dot(diff);
+					
+					if (t > 0) {
+						btScalar dotVV = v.dot(v);
+						if (t < dotVV) {
+							t /= dotVV;
+							diff -= t*v;
+							m_cachedBC.m_usedVertices.usedVertexA = true;
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						} else {
+							t = 1;
+							diff -= v;
+							//reduce to 1 point
+							m_cachedBC.m_usedVertices.usedVertexB = true;
+						}
+					} else
+					{
+						t = 0;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexA = true;
+					}
+					m_cachedBC.setBarycentricCoordinates(1-t,t);
+					nearest = from + t*v;
+
+					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					
+					reduceVertices(m_cachedBC.m_usedVertices);
+
+					m_cachedValidClosest = m_cachedBC.isValid();
+					break;
+			}
+		case 3: 
+			{ 
+				//closest point origin from triangle 
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 
+
+				const btVector3& a = m_simplexVectorW[0]; 
+				const btVector3& b = m_simplexVectorW[1]; 
+				const btVector3& c = m_simplexVectorW[2]; 
+
+				closestPtPointTriangle(p,a,b,c,m_cachedBC); 
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 
+				m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 
+				m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 
+
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 
+				m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 
+				m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 
+
+				m_cachedV = m_cachedP1-m_cachedP2; 
+
+				reduceVertices (m_cachedBC.m_usedVertices); 
+				m_cachedValidClosest = m_cachedBC.isValid(); 
+
+				break; 
+			}
+		case 4:
+			{
+
+				
+				btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
+				
+				const btVector3& a = m_simplexVectorW[0];
+				const btVector3& b = m_simplexVectorW[1];
+				const btVector3& c = m_simplexVectorW[2];
+				const btVector3& d = m_simplexVectorW[3];
+
+				bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+				if (hasSeperation)
+				{
+
+					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+					m_cachedV = m_cachedP1-m_cachedP2;
+					reduceVertices (m_cachedBC.m_usedVertices);
+				} else
+				{
+//					printf("sub distance got penetration\n");
+
+					if (m_cachedBC.m_degenerate)
+					{
+						m_cachedValidClosest = false;
+					} else
+					{
+						m_cachedValidClosest = true;
+						//degenerate case == false, penetration = true + zero
+						m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+					}
+					break;
+				}
+
+				m_cachedValidClosest = m_cachedBC.isValid();
+
+				//closest point origin from tetrahedron
+				break;
+			}
+		default:
+			{
+				m_cachedValidClosest = false;
+			}
+		};
+	}
+
+	return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool btVoronoiSimplexSolver::closest(btVector3& v)
+{
+	bool succes = updateClosestVectorAndPoints();
+	v = m_cachedV;
+	return succes;
+}
+
+
+
+btScalar btVoronoiSimplexSolver::maxVertex()
+{
+	int i, numverts = numVertices();
+	btScalar maxV = btScalar(0.);
+	for (i=0;i<numverts;i++)
+	{
+		btScalar curLen2 = m_simplexVectorW[i].length2();
+		if (maxV < curLen2)
+			maxV = curLen2;
+	}
+	return maxV;
+}
+
+
+
+	//return the current simplex
+int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
+{
+	int i;
+	for (i=0;i<numVertices();i++)
+	{
+		yBuf[i] = m_simplexVectorW[i];
+		pBuf[i] = m_simplexPointsP[i];
+		qBuf[i] = m_simplexPointsQ[i];
+	}
+	return numVertices();
+}
+
+
+
+
+bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
+{
+	bool found = false;
+	int i, numverts = numVertices();
+	//btScalar maxV = btScalar(0.);
+	
+	//w is in the current (reduced) simplex
+	for (i=0;i<numverts;i++)
+	{
+		if (m_simplexVectorW[i] == w)
+			found = true;
+	}
+
+	//check in case lastW is already removed
+	if (w == m_lastW)
+		return true;
+    	
+	return found;
+}
+
+void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
+{
+	v = m_cachedV;
+}
+
+
+bool btVoronoiSimplexSolver::emptySimplex() const 
+{
+	return (numVertices() == 0);
+
+}
+
+void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) 
+{
+	updateClosestVectorAndPoints();
+	p1 = m_cachedP1;
+	p2 = m_cachedP2;
+
+}
+
+
+
+
+bool	btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)
+{
+	result.m_usedVertices.reset();
+
+    // Check if P in vertex region outside A
+    btVector3 ab = b - a;
+    btVector3 ac = c - a;
+    btVector3 ap = p - a;
+    btScalar d1 = ab.dot(ap);
+    btScalar d2 = ac.dot(ap);
+    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
+	{
+		result.m_closestPointOnSimplex = a;
+		result.m_usedVertices.usedVertexA = true;
+		result.setBarycentricCoordinates(1,0,0);
+		return true;// a; // barycentric coordinates (1,0,0)
+	}
+
+    // Check if P in vertex region outside B
+    btVector3 bp = p - b;
+    btScalar d3 = ab.dot(bp);
+    btScalar d4 = ac.dot(bp);
+    if (d3 >= btScalar(0.0) && d4 <= d3) 
+	{
+		result.m_closestPointOnSimplex = b;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(0,1,0);
+
+		return true; // b; // barycentric coordinates (0,1,0)
+	}
+    // Check if P in edge region of AB, if so return projection of P onto AB
+    btScalar vc = d1*d4 - d3*d2;
+    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
+        btScalar v = d1 / (d1 - d3);
+		result.m_closestPointOnSimplex = a + v * ab;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexB = true;
+		result.setBarycentricCoordinates(1-v,v,0);
+		return true;
+        //return a + v * ab; // barycentric coordinates (1-v,v,0)
+    }
+
+    // Check if P in vertex region outside C
+    btVector3 cp = p - c;
+    btScalar d5 = ab.dot(cp);
+    btScalar d6 = ac.dot(cp);
+    if (d6 >= btScalar(0.0) && d5 <= d6) 
+	{
+		result.m_closestPointOnSimplex = c;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,0,1);
+		return true;//c; // barycentric coordinates (0,0,1)
+	}
+
+    // Check if P in edge region of AC, if so return projection of P onto AC
+    btScalar vb = d5*d2 - d1*d6;
+    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
+        btScalar w = d2 / (d2 - d6);
+		result.m_closestPointOnSimplex = a + w * ac;
+		result.m_usedVertices.usedVertexA = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(1-w,0,w);
+		return true;
+        //return a + w * ac; // barycentric coordinates (1-w,0,w)
+    }
+
+    // Check if P in edge region of BC, if so return projection of P onto BC
+    btScalar va = d3*d6 - d5*d4;
+    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
+        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		
+		result.m_closestPointOnSimplex = b + w * (c - b);
+		result.m_usedVertices.usedVertexB = true;
+		result.m_usedVertices.usedVertexC = true;
+		result.setBarycentricCoordinates(0,1-w,w);
+		return true;		
+       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+    }
+
+    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+    btScalar denom = btScalar(1.0) / (va + vb + vc);
+    btScalar v = vb * denom;
+    btScalar w = vc * denom;
+    
+	result.m_closestPointOnSimplex = a + ab * v + ac * w;
+	result.m_usedVertices.usedVertexA = true;
+	result.m_usedVertices.usedVertexB = true;
+	result.m_usedVertices.usedVertexC = true;
+	result.setBarycentricCoordinates(1-v-w,v,w);
+	
+	return true;
+//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
+{
+	btVector3 normal = (b-a).cross(c-a);
+
+    btScalar signp = (p - a).dot(normal); // [AP AB AC]
+    btScalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+	{
+		return -1;
+	}
+#else
+	if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
+	{
+//		printf("affine dependent/degenerate\n");//
+		return -1;
+	}
+#endif
+
+#endif
+	// Points on opposite sides if expression signs are opposite
+    return signp * signd < btScalar(0.);
+}
+
+
+bool	btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)
+{
+	btSubSimplexClosestResult tempResult;
+
+    // Start out assuming point inside all halfspaces, so closest to itself
+	finalResult.m_closestPointOnSimplex = p;
+	finalResult.m_usedVertices.reset();
+    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexB = true;
+	finalResult.m_usedVertices.usedVertexC = true;
+	finalResult.m_usedVertices.usedVertexD = true;
+
+    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+  	int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+	int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+   {
+	   finalResult.m_degenerate = true;
+	   return false;
+   }
+
+   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	 {
+		 return false;
+	 }
+
+
+    btScalar bestSqDist = FLT_MAX;
+    // If point outside face abc then compute closest point on abc
+	if (pointOutsideABC) 
+	{
+        closestPtPointTriangle(p, a, b, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		
+        btScalar sqDist = (q - p).dot( q - p);
+        // Update best closest point if (squared) distance is less than current best
+        if (sqDist < bestSqDist) {
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			//convert result bitmask!
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC],
+					0
+			);
+
+		}
+    }
+  
+
+	// Repeat test for face acd
+	if (pointOutsideACD) 
+	{
+        closestPtPointTriangle(p, a, c, d,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					0,
+					tempResult.m_barycentricCoords[VERTB],
+					tempResult.m_barycentricCoords[VERTC]
+			);
+
+		}
+    }
+    // Repeat test for face adb
+
+	
+	if (pointOutsideADB)
+	{
+		closestPtPointTriangle(p, a, d, b,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+			
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+			finalResult.setBarycentricCoordinates(
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					0,
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+    // Repeat test for face bdc
+    
+
+	if (pointOutsideBDC)
+	{
+        closestPtPointTriangle(p, b, d, c,tempResult);
+		btPoint3 q = tempResult.m_closestPointOnSimplex;
+		//convert result bitmask!
+        btScalar sqDist = (q - p).dot( q - p);
+        if (sqDist < bestSqDist) 
+		{
+			bestSqDist = sqDist;
+			finalResult.m_closestPointOnSimplex = q;
+			finalResult.m_usedVertices.reset();
+			//
+			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+
+			finalResult.setBarycentricCoordinates(
+					0,
+					tempResult.m_barycentricCoords[VERTA],
+					tempResult.m_barycentricCoords[VERTC],
+					tempResult.m_barycentricCoords[VERTB]
+			);
+
+		}
+    }
+
+	//help! we ended up full !
+	
+	if (finalResult.m_usedVertices.usedVertexA &&
+		finalResult.m_usedVertices.usedVertexB &&
+		finalResult.m_usedVertices.usedVertexC &&
+		finalResult.m_usedVertices.usedVertexD) 
+	{
+		return true;
+	}
+
+    return true;
+}
+
Index: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h	(revision 1963)
@@ -0,0 +1,157 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef btVoronoiSimplexSolver_H
+#define btVoronoiSimplexSolver_H
+
+#include "btSimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct btUsageBitfield{
+	btUsageBitfield()
+	{
+		reset();
+	}
+
+	void reset()
+	{
+		usedVertexA = false;
+		usedVertexB = false;
+		usedVertexC = false;
+		usedVertexD = false;
+	}
+	unsigned short usedVertexA	: 1;
+	unsigned short usedVertexB	: 1;
+	unsigned short usedVertexC	: 1;
+	unsigned short usedVertexD	: 1;
+	unsigned short unused1		: 1;
+	unsigned short unused2		: 1;
+	unsigned short unused3		: 1;
+	unsigned short unused4		: 1;
+};
+
+
+struct	btSubSimplexClosestResult
+{
+	btPoint3	m_closestPointOnSimplex;
+	//MASK for m_usedVertices
+	//stores the simplex vertex-usage, using the MASK, 
+	// if m_usedVertices & MASK then the related vertex is used
+	btUsageBitfield	m_usedVertices;
+	btScalar	m_barycentricCoords[4];
+	bool m_degenerate;
+
+	void	reset()
+	{
+		m_degenerate = false;
+		setBarycentricCoordinates();
+		m_usedVertices.reset();
+	}
+	bool	isValid()
+	{
+		bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
+			(m_barycentricCoords[1] >= btScalar(0.)) &&
+			(m_barycentricCoords[2] >= btScalar(0.)) &&
+			(m_barycentricCoords[3] >= btScalar(0.));
+
+
+		return valid;
+	}
+	void	setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
+	{
+		m_barycentricCoords[0] = a;
+		m_barycentricCoords[1] = b;
+		m_barycentricCoords[2] = c;
+		m_barycentricCoords[3] = d;
+	}
+
+};
+
+/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+#ifdef NO_VIRTUAL_INTERFACE
+class btVoronoiSimplexSolver
+#else
+class btVoronoiSimplexSolver : public btSimplexSolverInterface
+#endif
+{
+public:
+
+	int	m_numVertices;
+
+	btVector3	m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+	btPoint3	m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+	btPoint3	m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+	
+
+	btPoint3	m_cachedP1;
+	btPoint3	m_cachedP2;
+	btVector3	m_cachedV;
+	btVector3	m_lastW;
+	bool		m_cachedValidClosest;
+
+	btSubSimplexClosestResult m_cachedBC;
+
+	bool	m_needsUpdate;
+	
+	void	removeVertex(int index);
+	void	reduceVertices (const btUsageBitfield& usedVerts);
+	bool	updateClosestVectorAndPoints();
+
+	bool	closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult);
+	int		pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d);
+	bool	closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result);
+
+public:
+
+	 void reset();
+
+	 void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
+
+
+	 bool closest(btVector3& v);
+
+	 btScalar maxVertex();
+
+	 bool fullSimplex() const
+	 {
+		 return (m_numVertices == 4);
+	 }
+
+	 int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
+
+	 bool inSimplex(const btVector3& w);
+	
+	 void backup_closest(btVector3& v) ;
+
+	 bool emptySimplex() const ;
+
+	 void compute_points(btPoint3& p1, btPoint3& p2) ;
+
+	 int numVertices() const 
+	 {
+		 return m_numVertices;
+	 }
+
+
+};
+
+#endif //VoronoiSimplexSolver
Index: code/branches/physics/src/bullet/BulletDynamics/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,32 @@
+ADD_LIBRARY(LibBulletDynamics
+
+	ConstraintSolver/btContactConstraint.cpp
+	ConstraintSolver/btContactConstraint.h
+	ConstraintSolver/btConeTwistConstraint.cpp
+	ConstraintSolver/btConeTwistConstraint.h
+	ConstraintSolver/btGeneric6DofConstraint.cpp
+	ConstraintSolver/btGeneric6DofConstraint.h
+	ConstraintSolver/btHingeConstraint.cpp
+	ConstraintSolver/btHingeConstraint.h
+	ConstraintSolver/btPoint2PointConstraint.cpp
+	ConstraintSolver/btPoint2PointConstraint.h
+	ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+	ConstraintSolver/btSequentialImpulseConstraintSolver.h
+	ConstraintSolver/btSliderConstraint.cpp
+	ConstraintSolver/btSliderConstraint.h
+	ConstraintSolver/btSolve2LinearConstraint.cpp
+	ConstraintSolver/btSolve2LinearConstraint.h
+	ConstraintSolver/btTypedConstraint.cpp
+	ConstraintSolver/btTypedConstraint.h
+	Dynamics/Bullet-C-API.cpp
+	Dynamics/btDiscreteDynamicsWorld.cpp
+	Dynamics/btDiscreteDynamicsWorld.h
+	Dynamics/btSimpleDynamicsWorld.cpp
+	Dynamics/btSimpleDynamicsWorld.h
+	Dynamics/btRigidBody.cpp
+	Dynamics/btRigidBody.h
+	Vehicle/btRaycastVehicle.cpp
+	Vehicle/btRaycastVehicle.h
+	Vehicle/btWheelInfo.cpp
+	Vehicle/btWheelInfo.h
+)
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 1963)
@@ -0,0 +1,289 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+#include "btConeTwistConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMinMax.h"
+#include <new>
+
+btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+{
+}
+
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+											 const btTransform& rbAFrame,const btTransform& rbBFrame)
+											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+											 m_angularOnly(false)
+{
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
+											 m_angularOnly(false)
+{
+	m_rbBFrame = m_rbAFrame;
+	
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	
+}			
+
+void	btConeTwistConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	//set bias, sign, clear accumulator
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	m_accTwistLimitImpulse = btScalar(0.);
+	m_accSwingLimitImpulse = btScalar(0.);
+
+	if (!m_angularOnly)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+		for (int i=0;i<3;i++)
+		{
+			new (&m_jac[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+		}
+	}
+
+	btVector3 b1Axis1,b1Axis2,b1Axis3;
+	btVector3 b2Axis1,b2Axis2;
+
+	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
+	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
+
+	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+
+	btScalar swx=btScalar(0.),swy = btScalar(0.);
+	btScalar thresh = btScalar(10.);
+	btScalar fact;
+
+	// Get Frame into world space
+	if (m_swingSpan1 >= btScalar(0.05f))
+	{
+		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
+//		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+		swx = b2Axis1.dot(b1Axis1);
+		swy = b2Axis1.dot(b1Axis2);
+		swing1  = btAtan2Fast(swy, swx);
+		fact = (swy*swy + swx*swx) * thresh * thresh;
+		fact = fact / (fact + btScalar(1.0));
+		swing1 *= fact; 
+
+	}
+
+	if (m_swingSpan2 >= btScalar(0.05f))
+	{
+		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
+//		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+		swx = b2Axis1.dot(b1Axis1);
+		swy = b2Axis1.dot(b1Axis3);
+		swing2  = btAtan2Fast(swy, swx);
+		fact = (swy*swy + swx*swx) * thresh * thresh;
+		fact = fact / (fact + btScalar(1.0));
+		swing2 *= fact; 
+	}
+
+	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
+	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
+	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
+
+	if (EllipseAngle > 1.0f)
+	{
+		m_swingCorrection = EllipseAngle-1.0f;
+		m_solveSwingLimit = true;
+		
+		// Calculate necessary axis & factors
+		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis.normalize();
+
+		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+		m_swingAxis *= swingAxisSign;
+
+		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+
+	}
+
+	// Twist limits
+	if (m_twistSpan >= btScalar(0.))
+	{
+		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
+		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
+		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
+		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+
+		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		if (twist <= -m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = -(twist + m_twistSpan);
+			m_solveTwistLimit = true;
+
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+			m_twistAxis *= -1.0f;
+
+			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+		}	else
+			if (twist >  m_twistSpan*lockedFreeFactor)
+			{
+				m_twistCorrection = (twist - m_twistSpan);
+				m_solveTwistLimit = true;
+
+				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+				m_twistAxis.normalize();
+
+				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+			}
+	}
+}
+
+void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
+{
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+	btScalar tau = btScalar(0.3);
+
+	//linear part
+	if (!m_angularOnly)
+	{
+		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+
+		for (int i=0;i<3;i++)
+		{		
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			btVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		}
+	}
+	
+	{
+		///solve angular part
+		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+
+		// solve swing limit
+		if (m_solveSwingLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
+			btScalar impulseMag = amplitude * m_kSwing;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accSwingLimitImpulse;
+			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+			impulseMag = m_accSwingLimitImpulse - temp;
+
+			btVector3 impulse = m_swingAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+
+		// solve twist limit
+		if (m_solveTwistLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
+			btScalar impulseMag = amplitude * m_kTwist;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accTwistLimitImpulse;
+			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+			impulseMag = m_accTwistLimitImpulse - temp;
+
+			btVector3 impulse = m_twistAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+	
+	}
+
+}
+
+void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 1963)
@@ -0,0 +1,126 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+
+#ifndef CONETWISTCONSTRAINT_H
+#define CONETWISTCONSTRAINT_H
+
+#include "../../LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
+class btConeTwistConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+
+	btTransform m_rbAFrame; 
+	btTransform m_rbBFrame;
+
+	btScalar	m_limitSoftness;
+	btScalar	m_biasFactor;
+	btScalar	m_relaxationFactor;
+
+	btScalar	m_swingSpan1;
+	btScalar	m_swingSpan2;
+	btScalar	m_twistSpan;
+
+	btVector3   m_swingAxis;
+	btVector3	m_twistAxis;
+
+	btScalar	m_kSwing;
+	btScalar	m_kTwist;
+
+	btScalar	m_twistLimitSign;
+	btScalar	m_swingCorrection;
+	btScalar	m_twistCorrection;
+
+	btScalar	m_accSwingLimitImpulse;
+	btScalar	m_accTwistLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_solveTwistLimit;
+	bool		m_solveSwingLimit;
+
+	
+public:
+
+	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
+	
+	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	btConeTwistConstraint();
+
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_swingSpan1 = _swingSpan1;
+		m_swingSpan2 = _swingSpan2;
+		m_twistSpan  = _twistSpan;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+	}
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveTwistLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline int getSolveSwingLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline btScalar getTwistLimitSign()
+	{
+		return m_twistLimitSign;
+	}
+
+};
+
+#endif //CONETWISTCONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h	(revision 1963)
@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONSTRAINT_SOLVER_H
+#define CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btScalar.h"
+
+class btPersistentManifold;
+class btRigidBody;
+class btCollisionObject;
+class btTypedConstraint;
+struct btContactSolverInfo;
+struct btBroadphaseProxy;
+class btIDebugDraw;
+class btStackAlloc;
+class	btDispatcher;
+/// btConstraintSolver provides solver interface
+class btConstraintSolver
+{
+
+public:
+
+	virtual ~btConstraintSolver() {}
+	
+	virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
+
+	virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset() = 0;
+};
+
+
+
+
+#endif //CONSTRAINT_SOLVER_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 1963)
@@ -0,0 +1,429 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+#define ASSERT2 assert
+
+#define USE_INTERNAL_APPLY_IMPULSE 1
+
+
+//bilateral constraint between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+{
+	(void)timeStep;
+	(void)distance;
+
+
+	btScalar normalLenSqr = normal.length2();
+	ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
+	if (normalLenSqr > btScalar(1.1))
+	{
+		impulse = btScalar(0.);
+		return;
+	}
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	//this jacobian entry could be re-used for all iterations
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	
+
+	  btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+		body2.getCenterOfMassTransform().getBasis().transpose(),
+		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
+		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+
+	btScalar jacDiagAB = jac.getDiagonal();
+	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
+	
+	  btScalar rel_vel = jac.getRelativeVelocity(
+		body1.getLinearVelocity(),
+		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
+		body2.getLinearVelocity(),
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
+	btScalar a;
+	a=jacDiagABInv;
+
+
+	rel_vel = normal.dot(vel);
+	
+	//todo: move this into proper structure
+	btScalar contactDamping = btScalar(0.2);
+
+#ifdef ONLY_USE_LINEAR_MASS
+	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
+	impulse = - contactDamping * rel_vel * massTerm;
+#else	
+	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+	impulse = velocityImpulse;
+#endif
+}
+
+
+
+//response  between two dynamic objects with friction
+btScalar resolveSingleCollision(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	//constant over all iterations
+	btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
+
+	// btScalar damping = solverInfo.m_damping ;
+	btScalar Kerp = solverInfo.m_erp;
+	btScalar Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	return normalImpulse;
+}
+
+
+btScalar resolveSingleFriction(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	(void)solverInfo;
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	btScalar combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	
+	if (cpd->m_appliedImpulse>btScalar(0.))
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		// 1st tangent
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	
+		btScalar j1,j2;
+
+		{
+				
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
+			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
+			btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
+			btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
+			j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
+
+		}
+		{
+			// 2nd tangent
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
+			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
+			btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
+			btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
+			j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
+		}
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
+	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+
+	} 
+	return cpd->m_appliedImpulse;
+}
+
+
+btScalar resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo);
+
+btScalar resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	(void)solverInfo;
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	btScalar combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	//if (contactPoint.m_appliedImpulse>btScalar(0.))
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		{
+			// 1st tangent
+			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			btVector3 vel = vel1 - vel2;
+			
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
+			btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
+			btSetMin(total, limit);
+			btSetMax(total, -limit);
+			j = total - cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
+		}
+
+				
+		{
+			// 2nd tangent
+			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+			btVector3 vel = vel1 - vel2;
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
+			btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
+			btSetMin(total, limit);
+			btSetMax(total, -limit);
+			j = total - cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = total;
+			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
+			body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
+		}
+	} 
+	return cpd->m_appliedImpulse;
+}
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+btScalar resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
+
+	//btScalar damping = solverInfo.m_damping ;
+	btScalar Kerp = solverInfo.m_erp;
+	btScalar Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else //USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	{
+		//friction
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	  
+		rel_vel = normal.dot(vel);
+
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length();
+
+		btScalar combinedFriction = cpd->m_friction;
+
+		if (cpd->m_appliedImpulse > 0)
+		if (lat_rel_vel > SIMD_EPSILON)
+		{
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
+
+			btSetMin(friction_impulse, normal_impulse);
+			btSetMax(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+
+
+	return normalImpulse;
+}
+
+btScalar resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo);
+
+btScalar resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+	(void)contactPoint;
+	(void)body1;
+	(void)body2;
+	(void)solverInfo;
+
+
+	return btScalar(0.);
+};
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h	(revision 1963)
@@ -0,0 +1,122 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_CONSTRAINT_H
+#define CONTACT_CONSTRAINT_H
+
+//todo: make into a proper class working with the iterative constraint solver
+
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+struct btContactSolverInfo;
+class btManifoldPoint;
+
+enum {
+	DEFAULT_CONTACT_SOLVER_TYPE=0,
+	CONTACT_SOLVER_TYPE1,
+	CONTACT_SOLVER_TYPE2,
+	USER_CONTACT_SOLVER_TYPE1,
+	MAX_CONTACT_SOLVER_TYPES
+};
+
+
+typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
+									 btRigidBody& body2,
+									 class btManifoldPoint& contactPoint,
+									 const btContactSolverInfo& info);
+
+///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
+struct btConstraintPersistentData
+{
+	inline btConstraintPersistentData()
+	:m_appliedImpulse(btScalar(0.)),
+	m_prevAppliedImpulse(btScalar(0.)),
+	m_accumulatedTangentImpulse0(btScalar(0.)),
+	m_accumulatedTangentImpulse1(btScalar(0.)),
+	m_jacDiagABInv(btScalar(0.)),
+	m_persistentLifeTime(0),
+	m_restitution(btScalar(0.)),
+	m_friction(btScalar(0.)),
+	m_penetration(btScalar(0.)),
+	m_contactSolverFunc(0),
+	m_frictionSolverFunc(0)
+	{
+	}
+	
+					
+				/// total applied impulse during most recent frame
+			btScalar	m_appliedImpulse;
+			btScalar	m_prevAppliedImpulse;
+			btScalar	m_accumulatedTangentImpulse0;
+			btScalar	m_accumulatedTangentImpulse1;
+			
+			btScalar	m_jacDiagABInv;
+			btScalar	m_jacDiagABInvTangent0;
+			btScalar	m_jacDiagABInvTangent1;
+			int		m_persistentLifeTime;
+			btScalar	m_restitution;
+			btScalar	m_friction;
+			btScalar	m_penetration;
+			btVector3	m_frictionWorldTangential0;
+			btVector3	m_frictionWorldTangential1;
+
+			btVector3	m_frictionAngularComponent0A;
+			btVector3	m_frictionAngularComponent0B;
+			btVector3	m_frictionAngularComponent1A;
+			btVector3	m_frictionAngularComponent1B;
+
+			//some data doesn't need to be persistent over frames: todo: clean/reuse this
+			btVector3	m_angularComponentA;
+			btVector3	m_angularComponentB;
+		
+			ContactSolverFunc	m_contactSolverFunc;
+			ContactSolverFunc	m_frictionSolverFunc;
+
+};
+
+///bilateral constraint between two dynamic objects
+///positive distance = separation, negative distance = penetration
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
+
+
+///contact constraint resolution:
+///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
+///positive distance = separation, negative distance = penetration
+btScalar resolveSingleCollision(
+	btRigidBody& body1,
+	btRigidBody& body2,
+		btManifoldPoint& contactPoint,
+		 const btContactSolverInfo& info);
+
+btScalar resolveSingleFriction(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo
+		);
+
+
+
+btScalar resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo
+		);
+
+#endif //CONTACT_CONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 1963)
@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_SOLVER_INFO
+#define CONTACT_SOLVER_INFO
+
+enum	btSolverMode
+{
+	SOLVER_RANDMIZE_ORDER = 1,
+	SOLVER_FRICTION_SEPARATE = 2,
+	SOLVER_USE_WARMSTARTING = 4,
+	SOLVER_CACHE_FRIENDLY = 8
+};
+
+struct btContactSolverInfoData
+{
+	
+
+	btScalar	m_tau;
+	btScalar	m_damping;
+	btScalar	m_friction;
+	btScalar	m_timeStep;
+	btScalar	m_restitution;
+	int		m_numIterations;
+	btScalar	m_maxErrorReduction;
+	btScalar	m_sor;
+	btScalar	m_erp;//used as Baumgarte factor
+	btScalar	m_erp2;//used in Split Impulse
+	int			m_splitImpulse;
+	btScalar	m_splitImpulsePenetrationThreshold;
+	btScalar	m_linearSlop;
+	btScalar	m_warmstartingFactor;
+
+	int			m_solverMode;
+	int	m_restingContactRestitutionThreshold;
+
+
+};
+
+struct btContactSolverInfo : public btContactSolverInfoData
+{
+
+	
+
+	inline btContactSolverInfo()
+	{
+		m_tau = btScalar(0.6);
+		m_damping = btScalar(1.0);
+		m_friction = btScalar(0.3);
+		m_restitution = btScalar(0.);
+		m_maxErrorReduction = btScalar(20.);
+		m_numIterations = 10;
+		m_erp = btScalar(0.2);
+		m_erp2 = btScalar(0.1);
+		m_sor = btScalar(1.3);
+		m_splitImpulse = false;
+		m_splitImpulsePenetrationThreshold = -0.02f;
+		m_linearSlop = btScalar(0.0);
+		m_warmstartingFactor=btScalar(0.85);
+		m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING;
+		m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
+	}
+};
+
+#endif //CONTACT_SOLVER_INFO
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 1963)
@@ -0,0 +1,528 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+2007-09-09
+Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+
+#include "btGeneric6DofConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
+static const int kAxisA[] = { 1, 0, 0 };
+static const int kAxisB[] = { 2, 2, 1 };
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
+{
+	int i = index%3;
+	int j = index/3;
+	return mat[i][j];
+}
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+//	// rot =  cy*cz          -cy*sz           sy
+//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+//
+
+		if (btGetMatrixElem(mat,2) < btScalar(1.0))
+		{
+			if (btGetMatrixElem(mat,2) > btScalar(-1.0))
+			{
+				xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+				xyz[1] = btAsin(btGetMatrixElem(mat,2));
+				xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+				return true;
+			}
+			else
+			{
+				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+				xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+				xyz[1] = -SIMD_HALF_PI;
+				xyz[2] = btScalar(0.0);
+				return false;
+			}
+		}
+		else
+		{
+			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+			xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = SIMD_HALF_PI;
+			xyz[2] = 0.0;
+
+		}
+
+
+	return false;
+}
+
+
+
+//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
+
+
+int btRotationalLimitMotor::testLimitValue(btScalar test_value)
+{
+	if(m_loLimit>m_hiLimit)
+	{
+		m_currentLimit = 0;//Free from violation
+		return 0;
+	}
+
+	if (test_value < m_loLimit)
+	{
+		m_currentLimit = 1;//low limit violation
+		m_currentLimitError =  test_value - m_loLimit;
+		return 1;
+	}
+	else if (test_value> m_hiLimit)
+	{
+		m_currentLimit = 2;//High limit violation
+		m_currentLimitError = test_value - m_hiLimit;
+		return 2;
+	};
+
+	m_currentLimit = 0;//Free from violation
+	return 0;
+	
+}
+
+
+btScalar btRotationalLimitMotor::solveAngularLimits(
+		btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+	 	btRigidBody * body0, btRigidBody * body1)
+{
+    if (needApplyTorques()==false) return 0.0f;
+
+    btScalar target_velocity = m_targetVelocity;
+    btScalar maxMotorForce = m_maxMotorForce;
+
+	//current error correction
+    if (m_currentLimit!=0)
+    {
+        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+        maxMotorForce = m_maxLimitForce;
+    }
+
+    maxMotorForce *= timeStep;
+
+    // current velocity difference
+    btVector3 vel_diff = body0->getAngularVelocity();
+    if (body1)
+    {
+        vel_diff -= body1->getAngularVelocity();
+    }
+
+
+
+    btScalar rel_vel = axis.dot(vel_diff);
+
+	// correction velocity
+    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+
+
+    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+    {
+        return 0.0f;//no need for applying force
+    }
+
+
+	// correction impulse
+    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+
+	// clip correction impulse
+    btScalar clippedMotorImpulse;
+
+    //todo: should clip against accumulated impulse
+    if (unclippedMotorImpulse>0.0f)
+    {
+        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+    }
+    else
+    {
+        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+    }
+
+
+	// sort with accumulated impulses
+    btScalar	lo = btScalar(-1e30);
+    btScalar	hi = btScalar(1e30);
+
+    btScalar oldaccumImpulse = m_accumulatedImpulse;
+    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+
+
+    btVector3 motorImp = clippedMotorImpulse * axis;
+
+
+    body0->applyTorqueImpulse(motorImp);
+    if (body1) body1->applyTorqueImpulse(-motorImp);
+
+    return clippedMotorImpulse;
+
+
+}
+
+//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+btScalar btTranslationalLimitMotor::solveLinearAxis(
+		btScalar timeStep,
+        btScalar jacDiagABInv,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
+        int limit_index,
+        const btVector3 & axis_normal_on_a,
+		const btVector3 & anchorPos)
+{
+
+///find relative velocity
+//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+    btVector3 vel = vel1 - vel2;
+
+    btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+/// apply displacement correction
+
+//positional error (zeroth order error)
+    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+    btScalar	lo = btScalar(-1e30);
+    btScalar	hi = btScalar(1e30);
+
+    btScalar minLimit = m_lowerLimit[limit_index];
+    btScalar maxLimit = m_upperLimit[limit_index];
+
+    //handle the limits
+    if (minLimit < maxLimit)
+    {
+        {
+            if (depth > maxLimit)
+            {
+                depth -= maxLimit;
+                lo = btScalar(0.);
+
+            }
+            else
+            {
+                if (depth < minLimit)
+                {
+                    depth -= minLimit;
+                    hi = btScalar(0.);
+                }
+                else
+                {
+                    return 0.0f;
+                }
+            }
+        }
+    }
+
+    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+    btScalar sum = oldNormalImpulse + normalImpulse;
+    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+    body1.applyImpulse( impulse_vector, rel_pos1);
+    body2.applyImpulse(-impulse_vector, rel_pos2);
+    return normalImpulse;
+}
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+        :btTypedConstraint(D6_CONSTRAINT_TYPE),
+		m_useLinearReferenceFrameA(true)
+{
+}
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+
+}
+
+
+
+
+
+void btGeneric6DofConstraint::calculateAngleInfo()
+{
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+
+	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+
+
+
+	// in euler angle mode we do not actually constrain the angular velocity
+  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+  //
+  //    to get			constrain w2-w1 along		...not
+  //    ------			---------------------		------
+  //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+  //    d(angle[1])/dt = 0	ax[1]
+  //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+  //
+  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+  // to prove the result for angle[0], write the expression for angle[0] from
+  // GetInfo1 then take the derivative. to prove this for angle[2] it is
+  // easier to take the euler rate expression for d(angle[2])/dt with respect
+  // to the components of w and set that to 0.
+
+	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+	m_calculatedAxis[1] = axis2.cross(axis0);
+	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+
+//    if(m_debugDrawer)
+//    {
+//
+//    	char buff[300];
+//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+//		m_calculatedAxisAngleDiff[0],
+//		m_calculatedAxisAngleDiff[1],
+//		m_calculatedAxisAngleDiff[2]);
+//    	m_debugDrawer->reportErrorWarning(buff);
+//    }
+
+}
+
+void btGeneric6DofConstraint::calculateTransforms()
+{
+    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+
+    calculateAngleInfo();
+}
+
+
+void btGeneric6DofConstraint::buildLinearJacobian(
+    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+    new (&jacLinear) btJacobianEntry(
+        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+        pivotAInW - m_rbA.getCenterOfMassPosition(),
+        pivotBInW - m_rbB.getCenterOfMassPosition(),
+        normalWorld,
+        m_rbA.getInvInertiaDiagLocal(),
+        m_rbA.getInvMass(),
+        m_rbB.getInvInertiaDiagLocal(),
+        m_rbB.getInvMass());
+
+}
+
+void btGeneric6DofConstraint::buildAngularJacobian(
+    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+    new (&jacAngular)	btJacobianEntry(jointAxisW,
+                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbA.getInvInertiaDiagLocal(),
+                                      m_rbB.getInvInertiaDiagLocal());
+
+}
+
+bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
+{
+    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+
+    //test limits
+    m_angularLimits[axis_index].testLimitValue(angle);
+    return m_angularLimits[axis_index].needApplyTorques();
+}
+
+void btGeneric6DofConstraint::buildJacobian()
+{
+
+	// Clear accumulated impulses for the next simulation step
+    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+    int i;
+    for(i = 0; i < 3; i++)
+    {
+        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+    }
+    //calculates transform
+    calculateTransforms();
+
+//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+	calcAnchorPos();
+	btVector3 pivotAInW = m_AnchorPos;
+	btVector3 pivotBInW = m_AnchorPos;
+
+// not used here
+//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+    btVector3 normalWorld;
+    //linear part
+    for (i=0;i<3;i++)
+    {
+        if (m_linearLimits.isLimited(i))
+        {
+			if (m_useLinearReferenceFrameA)
+	            normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+			else
+	            normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
+
+            buildLinearJacobian(
+                m_jacLinear[i],normalWorld ,
+                pivotAInW,pivotBInW);
+
+        }
+    }
+
+    // angular part
+    for (i=0;i<3;i++)
+    {
+        //calculates error angle
+        if (testAngularLimitMotor(i))
+        {
+            normalWorld = this->getAxis(i);
+            // Create angular atom
+            buildAngularJacobian(m_jacAng[i],normalWorld);
+        }
+    }
+
+
+}
+
+
+void btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
+{
+    m_timeStep = timeStep;
+
+    //calculateTransforms();
+
+    int i;
+
+    // linear
+
+    btVector3 pointInA = m_calculatedTransformA.getOrigin();
+	btVector3 pointInB = m_calculatedTransformB.getOrigin();
+
+	btScalar jacDiagABInv;
+	btVector3 linear_axis;
+    for (i=0;i<3;i++)
+    {
+        if (m_linearLimits.isLimited(i))
+        {
+            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+			if (m_useLinearReferenceFrameA)
+	            linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+			else
+	            linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+
+            m_linearLimits.solveLinearAxis(
+            	m_timeStep,
+            	jacDiagABInv,
+            	m_rbA,pointInA,
+                m_rbB,pointInB,
+                i,linear_axis, m_AnchorPos);
+
+        }
+    }
+
+    // angular
+    btVector3 angular_axis;
+    btScalar angularJacDiagABInv;
+    for (i=0;i<3;i++)
+    {
+        if (m_angularLimits[i].needApplyTorques())
+        {
+
+			// get axis
+			angular_axis = getAxis(i);
+
+			angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+
+			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
+        }
+    }
+}
+
+void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+{
+    (void)timeStep;
+
+}
+
+btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
+{
+    return m_calculatedAxis[axis_index];
+}
+
+btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
+{
+    return m_calculatedAxisAngleDiff[axis_index];
+}
+
+void btGeneric6DofConstraint::calcAnchorPos(void)
+{
+	btScalar imA = m_rbA.getInvMass();
+	btScalar imB = m_rbB.getInvMass();
+	btScalar weight;
+	if(imB == btScalar(0.0))
+	{
+		weight = btScalar(1.0);
+	}
+	else
+	{
+		weight = imA / (imA + imB);
+	}
+	const btVector3& pA = m_calculatedTransformA.getOrigin();
+	const btVector3& pB = m_calculatedTransformB.getOrigin();
+	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
+	return;
+} // btGeneric6DofConstraint::calcAnchorPos()
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 1963)
@@ -0,0 +1,437 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+
+#ifndef GENERIC_6DOF_CONSTRAINT_H
+#define GENERIC_6DOF_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+//! Rotation Limit structure for generic joints
+class btRotationalLimitMotor
+{
+public:
+    //! limit_parameters
+    //!@{
+    btScalar m_loLimit;//!< joint limit
+    btScalar m_hiLimit;//!< joint limit
+    btScalar m_targetVelocity;//!< target motor velocity
+    btScalar m_maxMotorForce;//!< max force on motor
+    btScalar m_maxLimitForce;//!< max force on limit
+    btScalar m_damping;//!< Damping.
+    btScalar m_limitSoftness;//! Relaxation factor
+    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_bounce;//!< restitution factor
+    bool m_enableMotor;
+
+    //!@}
+
+    //! temp_variables
+    //!@{
+    btScalar m_currentLimitError;//!  How much is violated this limit
+    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
+    btScalar m_accumulatedImpulse;
+    //!@}
+
+    btRotationalLimitMotor()
+    {
+    	m_accumulatedImpulse = 0.f;
+        m_targetVelocity = 0;
+        m_maxMotorForce = 0.1f;
+        m_maxLimitForce = 300.0f;
+        m_loLimit = -SIMD_INFINITY;
+        m_hiLimit = SIMD_INFINITY;
+        m_ERP = 0.5f;
+        m_bounce = 0.0f;
+        m_damping = 1.0f;
+        m_limitSoftness = 0.5f;
+        m_currentLimit = 0;
+        m_currentLimitError = 0;
+        m_enableMotor = false;
+    }
+
+    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
+    {
+        m_targetVelocity = limot.m_targetVelocity;
+        m_maxMotorForce = limot.m_maxMotorForce;
+        m_limitSoftness = limot.m_limitSoftness;
+        m_loLimit = limot.m_loLimit;
+        m_hiLimit = limot.m_hiLimit;
+        m_ERP = limot.m_ERP;
+        m_bounce = limot.m_bounce;
+        m_currentLimit = limot.m_currentLimit;
+        m_currentLimitError = limot.m_currentLimitError;
+        m_enableMotor = limot.m_enableMotor;
+    }
+
+
+
+	//! Is limited
+    bool isLimited()
+    {
+    	if(m_loLimit>=m_hiLimit) return false;
+    	return true;
+    }
+
+	//! Need apply correction
+    bool needApplyTorques()
+    {
+    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
+    	return true;
+    }
+
+	//! calculates  error
+	/*!
+	calculates m_currentLimit and m_currentLimitError.
+	*/
+	int testLimitValue(btScalar test_value);
+
+	//! apply the correction impulses for two bodies
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
+
+
+};
+
+
+
+class btTranslationalLimitMotor
+{
+public:
+	btVector3 m_lowerLimit;//!< the constraint lower limits
+    btVector3 m_upperLimit;//!< the constraint upper limits
+    btVector3 m_accumulatedImpulse;
+    //! Linear_Limit_parameters
+    //!@{
+    btScalar	m_limitSoftness;//!< Softness for linear limit
+    btScalar	m_damping;//!< Damping for linear limit
+    btScalar	m_restitution;//! Bounce parameter for linear limit
+    //!@}
+
+    btTranslationalLimitMotor()
+    {
+    	m_lowerLimit.setValue(0.f,0.f,0.f);
+    	m_upperLimit.setValue(0.f,0.f,0.f);
+    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+
+    	m_limitSoftness = 0.7f;
+    	m_damping = btScalar(1.0f);
+    	m_restitution = btScalar(0.5f);
+    }
+
+    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
+    {
+    	m_lowerLimit = other.m_lowerLimit;
+    	m_upperLimit = other.m_upperLimit;
+    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+
+    	m_limitSoftness = other.m_limitSoftness ;
+    	m_damping = other.m_damping;
+    	m_restitution = other.m_restitution;
+    }
+
+    //! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+    inline bool	isLimited(int limitIndex)
+    {
+       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+    }
+
+
+    btScalar solveLinearAxis(
+    	btScalar timeStep,
+        btScalar jacDiagABInv,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
+        int limit_index,
+        const btVector3 & axis_normal_on_a,
+		const btVector3 & anchorPos);
+
+
+};
+
+/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/*!
+btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+currently this limit supports rotational motors<br>
+<ul>
+<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
+At this moment translational motors are not supported. May be in the future. </li>
+
+<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
+This is accessible through btGeneric6DofConstraint.getLimitMotor method,
+This brings support for limit parameters and motors. </li>
+
+<li> Angulars limits have these possible ranges:
+<table border=1 >
+<tr
+
+	<td><b>AXIS</b></td>
+	<td><b>MIN ANGLE</b></td>
+	<td><b>MAX ANGLE</b></td>
+	<td>X</td>
+		<td>-PI</td>
+		<td>PI</td>
+	<td>Y</td>
+		<td>-PI/2</td>
+		<td>PI/2</td>
+	<td>Z</td>
+		<td>-PI/2</td>
+		<td>PI/2</td>
+</tr>
+</table>
+</li>
+</ul>
+
+*/
+class btGeneric6DofConstraint : public btTypedConstraint
+{
+protected:
+
+	//! relative_frames
+    //!@{
+	btTransform	m_frameInA;//!< the constraint space w.r.t body A
+    btTransform	m_frameInB;//!< the constraint space w.r.t body B
+    //!@}
+
+    //! Jacobians
+    //!@{
+    btJacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
+    btJacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
+    //!@}
+
+	//! Linear_Limit_parameters
+    //!@{
+    btTranslationalLimitMotor m_linearLimits;
+    //!@}
+
+
+    //! hinge_parameters
+    //!@{
+    btRotationalLimitMotor m_angularLimits[3];
+	//!@}
+
+
+protected:
+    //! temporal variables
+    //!@{
+    btScalar m_timeStep;
+    btTransform m_calculatedTransformA;
+    btTransform m_calculatedTransformB;
+    btVector3 m_calculatedAxisAngleDiff;
+    btVector3 m_calculatedAxis[3];
+    
+	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+
+    bool	m_useLinearReferenceFrameA;
+    
+    //!@}
+
+    btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
+    {
+        btAssert(0);
+        (void) other;
+        return *this;
+    }
+
+
+
+    void buildLinearJacobian(
+        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+        const btVector3 & pivotAInW,const btVector3 & pivotBInW);
+
+    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+
+
+	//! calcs the euler angles between the two bodies.
+    void calculateAngleInfo();
+
+
+
+public:
+    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+
+    btGeneric6DofConstraint();
+
+	//! Calcs global transform of the offsets
+	/*!
+	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
+	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
+	*/
+    void calculateTransforms();
+
+	//! Gets the global transform of the offset for body A
+    /*!
+    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+    */
+    const btTransform & getCalculatedTransformA() const
+    {
+    	return m_calculatedTransformA;
+    }
+
+    //! Gets the global transform of the offset for body B
+    /*!
+    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+    */
+    const btTransform & getCalculatedTransformB() const
+    {
+    	return m_calculatedTransformB;
+    }
+
+    const btTransform & getFrameOffsetA() const
+    {
+    	return m_frameInA;
+    }
+
+    const btTransform & getFrameOffsetB() const
+    {
+    	return m_frameInB;
+    }
+
+
+    btTransform & getFrameOffsetA()
+    {
+    	return m_frameInA;
+    }
+
+    btTransform & getFrameOffsetB()
+    {
+    	return m_frameInB;
+    }
+
+
+	//! performs Jacobian calculation, and also calculates angle differences and axis
+    virtual void	buildJacobian();
+
+    virtual	void	solveConstraint(btScalar	timeStep);
+
+    void	updateRHS(btScalar	timeStep);
+
+	//! Get the rotation axis in global coordinates
+	/*!
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    btVector3 getAxis(int axis_index) const;
+
+    //! Get the relative Euler angle
+    /*!
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    btScalar getAngle(int axis_index) const;
+
+	//! Test angular limit.
+	/*!
+	Calculates angular correction and returns true if limit needs to be corrected.
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    bool testAngularLimitMotor(int axis_index);
+
+    void	setLinearLowerLimit(const btVector3& linearLower)
+    {
+    	m_linearLimits.m_lowerLimit = linearLower;
+    }
+
+    void	setLinearUpperLimit(const btVector3& linearUpper)
+    {
+    	m_linearLimits.m_upperLimit = linearUpper;
+    }
+
+    void	setAngularLowerLimit(const btVector3& angularLower)
+    {
+        m_angularLimits[0].m_loLimit = angularLower.getX();
+        m_angularLimits[1].m_loLimit = angularLower.getY();
+        m_angularLimits[2].m_loLimit = angularLower.getZ();
+    }
+
+    void	setAngularUpperLimit(const btVector3& angularUpper)
+    {
+        m_angularLimits[0].m_hiLimit = angularUpper.getX();
+        m_angularLimits[1].m_hiLimit = angularUpper.getY();
+        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+    }
+
+	//! Retrieves the angular limit informacion
+    btRotationalLimitMotor * getRotationalLimitMotor(int index)
+    {
+    	return &m_angularLimits[index];
+    }
+
+    //! Retrieves the  limit informacion
+    btTranslationalLimitMotor * getTranslationalLimitMotor()
+    {
+    	return &m_linearLimits;
+    }
+
+    //first 3 are linear, next 3 are angular
+    void setLimit(int axis, btScalar lo, btScalar hi)
+    {
+    	if(axis<3)
+    	{
+    		m_linearLimits.m_lowerLimit[axis] = lo;
+    		m_linearLimits.m_upperLimit[axis] = hi;
+    	}
+    	else
+    	{
+    		m_angularLimits[axis-3].m_loLimit = lo;
+    		m_angularLimits[axis-3].m_hiLimit = hi;
+    	}
+    }
+
+	//! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+    bool	isLimited(int limitIndex)
+    {
+    	if(limitIndex<3)
+    	{
+			return m_linearLimits.isLimited(limitIndex);
+
+    	}
+        return m_angularLimits[limitIndex-3].isLimited();
+    }
+
+    const btRigidBody& getRigidBodyA() const
+    {
+        return m_rbA;
+    }
+    const btRigidBody& getRigidBodyB() const
+    {
+        return m_rbB;
+    }
+
+	virtual void calcAnchorPos(void); // overridable
+
+};
+
+#endif //GENERIC_6DOF_CONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 1963)
@@ -0,0 +1,403 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btHingeConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMinMax.h"
+#include <new>
+
+
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
+m_enableAngularMotor(false)
+{
+}
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
+									 btVector3& axisInA,btVector3& axisInB)
+									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+									 m_angularOnly(false),
+									 m_enableAngularMotor(false)
+{
+	m_rbAFrame.getOrigin() = pivotInA;
+	
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+
+	btVector3 rbAxisA2;
+	btScalar projection = axisInA.dot(rbAxisA1);
+	if (projection >= 1.0f - SIMD_EPSILON) {
+		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+	} else if (projection <= -1.0f + SIMD_EPSILON) {
+		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
+	} else {
+		rbAxisA2 = axisInA.cross(rbAxisA1);
+		rbAxisA1 = rbAxisA2.cross(axisInA);
+	}
+
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
+	
+	m_rbBFrame.getOrigin() = pivotInB;
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+
+}
+
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+{
+
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	// fixed axis in worldspace
+	btVector3 rbAxisA1, rbAxisA2;
+	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
+
+	m_rbAFrame.getOrigin() = pivotInA;
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false)
+{
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}			
+
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false)
+{
+	///not providing rigidbody B means implicitly using worldspace for body B
+
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);	
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}
+
+void	btHingeConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	if (!m_angularOnly)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+		for (int i=0;i<3;i++)
+		{
+			new (&m_jac[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+		}
+	}
+
+	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+	//these two jointAxis require equal angular velocities for both bodies
+
+	//this is unused for now, it's a todo
+	btVector3 jointAxis0local;
+	btVector3 jointAxis1local;
+	
+	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		
+	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+	new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+	new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+
+	// Compute limit information
+	btScalar hingeAngle = getHingeAngle();  
+
+	//set bias, sign, clear accumulator
+	m_correction = btScalar(0.);
+	m_limitSign = btScalar(0.);
+	m_solveLimit = false;
+	m_accLimitImpulse = btScalar(0.);
+
+//	if (m_lowerLimit < m_upperLimit)
+	if (m_lowerLimit <= m_upperLimit)
+	{
+//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+		if (hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 
+//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+		else if (hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+
+	//Compute K = J*W*J' for hinge axis
+	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
+
+}
+
+void	btHingeConstraint::solveConstraint(btScalar	timeStep)
+{
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+	btScalar tau = btScalar(0.3);
+
+	//linear part
+	if (!m_angularOnly)
+	{
+		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+
+		for (int i=0;i<3;i++)
+		{		
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			btVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		}
+	}
+
+	
+	{
+		///solve angular part
+
+		// get axes in world space
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+
+		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+
+		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+		btVector3 velrelOrthog = angAorthog-angBorthog;
+		{
+			//solve orthogonal angular velocity correction
+			btScalar relaxation = btScalar(1.);
+			btScalar len = velrelOrthog.length();
+			if (len > btScalar(0.00001))
+			{
+				btVector3 normal = velrelOrthog.normalized();
+				btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+					getRigidBodyB().computeAngularImpulseDenominator(normal);
+				// scale for mass and relaxation
+				//todo:  expose this 0.9 factor to developer
+				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+			}
+
+			//solve angular positional correction
+			btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
+			btScalar len2 = angularError.length();
+			if (len2>btScalar(0.00001))
+			{
+				btVector3 normal2 = angularError.normalized();
+				btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal2);
+				angularError *= (btScalar(1.)/denom2) * relaxation;
+			}
+
+			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+			// solve limit
+			if (m_solveLimit)
+			{
+				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+				btScalar impulseMag = amplitude * m_kHinge;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accLimitImpulse;
+				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+				impulseMag = m_accLimitImpulse - temp;
+
+
+				btVector3 impulse = axisA * impulseMag * m_limitSign;
+				m_rbA.applyTorqueImpulse(impulse);
+				m_rbB.applyTorqueImpulse(-impulse);
+			}
+		}
+
+		//apply motor
+		if (m_enableAngularMotor) 
+		{
+			//todo: add limits too
+			btVector3 angularLimit(0,0,0);
+
+			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+			btScalar projRelVel = velrel.dot(axisA);
+
+			btScalar desiredMotorVel = m_motorTargetVelocity;
+			btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+			//todo: should clip against accumulated impulse
+			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+			btVector3 motorImp = clippedMotorImpulse * axisA;
+
+			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
+			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+			
+		}
+	}
+
+}
+
+void	btHingeConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+
+	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
+}
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 1963)
@@ -0,0 +1,158 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
+
+#ifndef HINGECONSTRAINT_H
+#define HINGECONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/// axis defines the orientation of the hinge axis
+class btHingeConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+
+	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransform m_rbBFrame;
+
+	btScalar	m_motorTargetVelocity;
+	btScalar	m_maxMotorImpulse;
+
+	btScalar	m_limitSoftness; 
+	btScalar	m_biasFactor; 
+	btScalar    m_relaxationFactor; 
+
+	btScalar    m_lowerLimit;	
+	btScalar    m_upperLimit;	
+	
+	btScalar	m_kHinge;
+
+	btScalar	m_limitSign;
+	btScalar	m_correction;
+
+	btScalar	m_accLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_enableAngularMotor;
+	bool		m_solveLimit;
+
+	
+public:
+
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+
+	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+	
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	btHingeConstraint();
+
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
+	{
+		m_enableAngularMotor  = enableMotor;
+		m_motorTargetVelocity = targetVelocity;
+		m_maxMotorImpulse = maxMotorImpulse;
+	}
+
+	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_lowerLimit = low;
+		m_upperLimit = high;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+
+	}
+
+	btScalar	getLowerLimit() const
+	{
+		return m_lowerLimit;
+	}
+
+	btScalar	getUpperLimit() const
+	{
+		return m_upperLimit;
+	}
+
+
+	btScalar getHingeAngle();
+
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveLimit()
+	{
+		return m_solveLimit;
+	}
+
+	inline btScalar getLimitSign()
+	{
+		return m_limitSign;
+	}
+
+	inline bool getAngularOnly() 
+	{ 
+		return m_angularOnly; 
+	}
+	inline bool getEnableAngularMotor() 
+	{ 
+		return m_enableAngularMotor; 
+	}
+	inline btScalar getMotorTargetVelosity() 
+	{ 
+		return m_motorTargetVelocity; 
+	}
+	inline btScalar getMaxMotorImpulse() 
+	{ 
+		return m_maxMotorImpulse; 
+	}
+
+};
+
+#endif //HINGECONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h	(revision 1963)
@@ -0,0 +1,156 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef JACOBIAN_ENTRY_H
+#define JACOBIAN_ENTRY_H
+
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the btJacobianEntry memory layout 16 bytes
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/// Jacobian entry is an abstraction that allows to describe constraints
+/// it can be used in combination with a constraint solver
+/// Can be used to relate the effect of an impulse to the constraint error
+class btJacobianEntry
+{
+public:
+	btJacobianEntry() {};
+	//constraint between two different rigidbodies
+	btJacobianEntry(
+		const btMatrix3x3& world2A,
+		const btMatrix3x3& world2B,
+		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& jointAxis,
+		const btVector3& inertiaInvA, 
+		const btScalar massInvA,
+		const btVector3& inertiaInvB,
+		const btScalar massInvB)
+		:m_linearJointAxis(jointAxis)
+	{
+		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	btJacobianEntry(const btVector3& jointAxis,
+		const btMatrix3x3& world2A,
+		const btMatrix3x3& world2B,
+		const btVector3& inertiaInvA,
+		const btVector3& inertiaInvB)
+		:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+	{
+		m_aJ= world2A*jointAxis;
+		m_bJ = world2B*-jointAxis;
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	btJacobianEntry(const btVector3& axisInA,
+		const btVector3& axisInB,
+		const btVector3& inertiaInvA,
+		const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+		, m_aJ(axisInA)
+		, m_bJ(-axisInB)
+	{
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	//constraint on one rigidbody
+	btJacobianEntry(
+		const btMatrix3x3& world2A,
+		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& jointAxis,
+		const btVector3& inertiaInvA, 
+		const btScalar massInvA)
+		:m_linearJointAxis(jointAxis)
+	{
+		m_aJ= world2A*(rel_pos1.cross(jointAxis));
+		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+		btAssert(m_Adiag > btScalar(0.0));
+	}
+
+	btScalar	getDiagonal() const { return m_Adiag; }
+
+	// for two constraints on the same rigidbody (for example vehicle friction)
+	btScalar	getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
+	{
+		const btJacobianEntry& jacA = *this;
+		btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+		btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+		return lin + ang;
+	}
+
+	
+
+	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+	btScalar	getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
+	{
+		const btJacobianEntry& jacA = *this;
+		btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
+		btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+		btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+		btVector3 lin0 = massInvA * lin ;
+		btVector3 lin1 = massInvB * lin;
+		btVector3 sum = ang0+ang1+lin0+lin1;
+		return sum[0]+sum[1]+sum[2];
+	}
+
+	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
+	{
+		btVector3 linrel = linvelA - linvelB;
+		btVector3 angvela  = angvelA * m_aJ;
+		btVector3 angvelb  = angvelB * m_bJ;
+		linrel *= m_linearJointAxis;
+		angvela += angvelb;
+		angvela += linrel;
+		btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		return rel_vel2 + SIMD_EPSILON;
+	}
+//private:
+
+	btVector3	m_linearJointAxis;
+	btVector3	m_aJ;
+	btVector3	m_bJ;
+	btVector3	m_0MinvJt;
+	btVector3	m_1MinvJt;
+	//Optimization: can be stored in the w/last component of one of the vectors
+	btScalar	m_Adiag;
+
+};
+
+#endif //JACOBIAN_ENTRY_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 1963)
@@ -0,0 +1,127 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btPoint2PointConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include <new>
+
+
+
+btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+{
+}
+
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+{
+
+}
+
+
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+{
+	
+}
+
+void	btPoint2PointConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	btVector3	normal(0,0,0);
+
+	for (int i=0;i<3;i++)
+	{
+		normal[i] = 1;
+		new (&m_jac[i]) btJacobianEntry(
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+			normal,
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbA.getInvMass(),
+			m_rbB.getInvInertiaDiagLocal(),
+			m_rbB.getInvMass());
+		normal[i] = 0;
+	}
+
+}
+
+void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
+{
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+	btVector3 normal(0,0,0);
+	
+
+//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+	for (int i=0;i<3;i++)
+	{		
+		normal[i] = 1;
+		btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+		//this jacobian entry could be re-used for all iterations
+		
+		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+		
+		btScalar rel_vel;
+		rel_vel = normal.dot(vel);
+
+	/*
+		//velocity error (first order error)
+		btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+														m_rbB.getLinearVelocity(),angvelB);
+	*/
+	
+		//positional error (zeroth order error)
+		btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+		
+		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+
+		btScalar impulseClamp = m_setting.m_impulseClamp;
+		if (impulseClamp > 0)
+		{
+			if (impulse < -impulseClamp) 
+				impulse = -impulseClamp;
+			if (impulse > impulseClamp) 
+				impulse = impulseClamp;
+		}
+
+		m_appliedImpulse+=impulse;
+		btVector3 impulse_vector = normal * impulse;
+		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+		m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		
+		normal[i] = 0;
+	}
+}
+
+void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 1963)
@@ -0,0 +1,91 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef POINT2POINTCONSTRAINT_H
+#define POINT2POINTCONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+struct	btConstraintSetting
+{
+	btConstraintSetting()	:
+		m_tau(btScalar(0.3)),
+		m_damping(btScalar(1.)),
+		m_impulseClamp(btScalar(0.))
+	{
+	}
+	btScalar		m_tau;
+	btScalar		m_damping;
+	btScalar		m_impulseClamp;
+};
+
+/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
+class btPoint2PointConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	
+	btVector3	m_pivotInA;
+	btVector3	m_pivotInB;
+	
+	
+	
+public:
+
+	btConstraintSetting	m_setting;
+
+	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
+
+	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
+
+	btPoint2PointConstraint();
+
+	virtual void	buildJacobian();
+
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	void	setPivotA(const btVector3& pivotA)
+	{
+		m_pivotInA = pivotA;
+	}
+
+	void	setPivotB(const btVector3& pivotB)
+	{
+		m_pivotInB = pivotB;
+	}
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+
+};
+
+#endif //POINT2POINTCONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 1963)
@@ -0,0 +1,1435 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
+
+#include "btSequentialImpulseConstraintSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btContactConstraint.h"
+#include "btSolve2LinearConstraint.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "btJacobianEntry.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include <new>
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btQuickprof.h"
+#include "btSolverBody.h"
+#include "btSolverConstraint.h"
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+int totalCpd = 0;
+
+int	gTotalContactPoints = 0;
+
+struct	btOrderIndex
+{
+	int	m_manifoldIndex;
+	int	m_pointIndex;
+};
+
+
+
+#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
+static btOrderIndex	gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+
+
+unsigned long btSequentialImpulseConstraintSolver::btRand2()
+{
+  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+  return m_btSeed2;
+}
+
+
+
+//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
+int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
+{
+  // seems good; xor-fold and modulus
+  const unsigned long un = static_cast<unsigned long>(n);
+  unsigned long r = btRand2();
+
+  // note: probably more aggressive than it needs to be -- might be
+  //       able to get away without one or two of the innermost branches.
+  if (un <= 0x00010000UL) {
+    r ^= (r >> 16);
+    if (un <= 0x00000100UL) {
+      r ^= (r >> 8);
+      if (un <= 0x00000010UL) {
+        r ^= (r >> 4);
+        if (un <= 0x00000004UL) {
+          r ^= (r >> 2);
+          if (un <= 0x00000002UL) {
+            r ^= (r >> 1);
+          }
+        }
+     }
+    }
+   }
+
+  return (int) (r % un);
+}
+
+
+
+
+bool  MyContactDestroyedCallback(void* userPersistentData);
+bool  MyContactDestroyedCallback(void* userPersistentData)
+{
+	assert (userPersistentData);
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
+	btAlignedFree(cpd);
+	totalCpd--;
+	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
+	return true;
+}
+
+
+
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
+	gContactDestroyedCallback = &MyContactDestroyedCallback;
+
+	//initialize default friction/contact funcs
+	int i,j;
+	for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
+		for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
+		{
+
+			m_contactDispatch[i][j] = resolveSingleCollision;
+			m_frictionDispatch[i][j] = resolveSingleFriction;
+		}
+}
+
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+
+}
+
+void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
+	btRigidBody* rb = btRigidBody::upcast(collisionObject);
+	if (rb)
+	{
+		solverBody->m_angularVelocity = rb->getAngularVelocity() ;
+		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
+		solverBody->m_friction = collisionObject->getFriction();
+		solverBody->m_invMass = rb->getInvMass();
+		solverBody->m_linearVelocity = rb->getLinearVelocity();
+		solverBody->m_originalBody = rb;
+		solverBody->m_angularFactor = rb->getAngularFactor();
+	} else
+	{
+		solverBody->m_angularVelocity.setValue(0,0,0);
+		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
+		solverBody->m_friction = collisionObject->getFriction();
+		solverBody->m_invMass = 0.f;
+		solverBody->m_linearVelocity.setValue(0,0,0);
+		solverBody->m_originalBody = 0;
+		solverBody->m_angularFactor = 1.f;
+	}
+	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
+	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
+}
+
+
+int		gNumSplitImpulseRecoveries = 0;
+
+btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+{
+	btScalar rest = restitution * -rel_vel;
+	return rest;
+}
+
+
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+//SIMD_FORCE_INLINE
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo)
+{
+        (void)solverInfo;
+
+		if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
+        {
+
+				gNumSplitImpulseRecoveries++;
+                btScalar normalImpulse;
+
+                //  Optimized version of projected relative velocity, use precomputed cross products with normal
+                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+                //      btVector3 vel = vel1 - vel2;
+                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+                btScalar rel_vel;
+                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
+                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
+                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
+                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
+
+                rel_vel = vel1Dotn-vel2Dotn;
+
+
+				btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
+                //      btScalar positionalError = contactConstraint.m_penetration;
+
+                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+                normalImpulse = penetrationImpulse+velocityImpulse;
+
+                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
+                btScalar sum = oldNormalImpulse + normalImpulse;
+                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
+
+				body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
+               
+				body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
+               
+        }
+
+}
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+
+btScalar resolveSingleCollisionCombinedCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo);
+
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleCollisionCombinedCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo)
+{
+	(void)solverInfo;
+
+	btScalar normalImpulse;
+
+	{
+
+		
+		//  Optimized version of projected relative velocity, use precomputed cross products with normal
+		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+		//	btVector3 vel = vel1 - vel2;
+		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+		btScalar rel_vel;
+		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+
+		rel_vel = vel1Dotn-vel2Dotn;
+
+		btScalar positionalError = 0.f;
+		if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
+		{
+ 			positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
+		}
+
+		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+		normalImpulse = penetrationImpulse+velocityImpulse;
+		
+		
+		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
+		btScalar sum = oldNormalImpulse + normalImpulse;
+		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
+
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
+				contactConstraint.m_angularComponentA,normalImpulse);
+		
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
+				contactConstraint.m_angularComponentB,-normalImpulse);
+	}
+
+	return normalImpulse;
+}
+
+
+#ifndef NO_FRICTION_TANGENTIALS
+
+btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo,
+	btScalar appliedNormalImpulse);
+
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo,
+	btScalar appliedNormalImpulse)
+{
+	(void)solverInfo;
+
+	
+	const btScalar combinedFriction = contactConstraint.m_friction;
+	
+	const btScalar limit = appliedNormalImpulse * combinedFriction;
+	
+	if (appliedNormalImpulse>btScalar(0.))
+	//friction
+	{
+		
+		btScalar j1;
+		{
+
+			btScalar rel_vel;
+			const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+						+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+			const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+			rel_vel = vel1Dotn-vel2Dotn;
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
+#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
+#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
+			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
+			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
+			
+			if (limit < contactConstraint.m_appliedImpulse)
+			{
+				contactConstraint.m_appliedImpulse = limit;
+			} else
+			{
+				if (contactConstraint.m_appliedImpulse < -limit)
+					contactConstraint.m_appliedImpulse = -limit;
+			}
+			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
+#else
+			if (limit < j1)
+			{
+				j1 = limit;
+			} else
+			{
+				if (j1 < -limit)
+					j1 = -limit;
+			}
+
+#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
+
+			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+
+			
+
+		}
+	
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
+		
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
+
+	} 
+	return 0.f;
+}
+
+
+#else
+
+//velocity + friction
+//response  between two dynamic objects with friction
+btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	btVector3 vel1;
+	btVector3 vel2;
+	btScalar normalImpulse(0.f);
+
+	{
+		const btVector3& normal = contactConstraint.m_contactNormal;
+		if (contactConstraint.m_penetration < 0.f)
+			return 0.f;
+
+
+		body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+		btVector3 vel = vel1 - vel2;
+		btScalar rel_vel;
+		rel_vel = normal.dot(vel);
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length2();
+
+		btScalar combinedFriction = contactConstraint.m_friction;
+		const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
+		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
+
+
+		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
+		{
+			lat_rel_vel = btSqrt(lat_rel_vel);
+
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
+
+			GEN_set_min(friction_impulse, normal_impulse);
+			GEN_set_max(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+	return normalImpulse;
+}
+
+#endif //NO_FRICTION_TANGENTIALS
+
+
+
+
+
+void	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+{
+
+	btRigidBody* body0=btRigidBody::upcast(colObj0);
+	btRigidBody* body1=btRigidBody::upcast(colObj1);
+
+	btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+	solverConstraint.m_contactNormal = normalAxis;
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
+	solverConstraint.m_frictionIndex = frictionIndex;
+
+	solverConstraint.m_friction = cp.m_combinedFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = btScalar(0.);
+	solverConstraint.m_appliedPushImpulse = 0.f;
+	solverConstraint.m_penetration = 0.f;
+	{
+		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+	}
+	{
+		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+	}
+
+#ifdef COMPUTE_IMPULSE_DENOM
+	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+#else
+	btVector3 vec;
+	btScalar denom0 = 0.f;
+	btScalar denom1 = 0.f;
+	if (body0)
+	{
+		vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+		denom0 = body0->getInvMass() + normalAxis.dot(vec);
+	}
+	if (body1)
+	{
+		vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+		denom1 = body1->getInvMass() + normalAxis.dot(vec);
+	}
+
+
+#endif //COMPUTE_IMPULSE_DENOM
+	btScalar denom = relaxation/(denom0+denom1);
+	solverConstraint.m_jacDiagABInv = denom;
+
+
+}
+
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+	BT_PROFILE("solveGroupCacheFriendlySetup");
+	(void)stackAlloc;
+	(void)debugDrawer;
+
+
+	if (!(numConstraints + numManifolds))
+	{
+//		printf("empty\n");
+		return 0.f;
+	}
+	btPersistentManifold* manifold = 0;
+	btCollisionObject* colObj0=0,*colObj1=0;
+
+	//btRigidBody* rb0=0,*rb1=0;
+
+
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+
+	BEGIN_PROFILE("refreshManifolds");
+
+	int i;
+	
+	
+
+	for (i=0;i<numManifolds;i++)
+	{
+		manifold = manifoldPtr[i];
+		rb1 = (btRigidBody*)manifold->getBody1();
+		rb0 = (btRigidBody*)manifold->getBody0();
+		
+		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+
+	}
+
+	END_PROFILE("refreshManifolds");
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
+	
+
+
+
+	//int sizeofSB = sizeof(btSolverBody);
+	//int sizeofSC = sizeof(btSolverConstraint);
+
+
+	//if (1)
+	{
+		//if m_stackAlloc, try to pack bodies/constraints to speed up solving
+//		btBlock*					sablock;
+//		sablock = stackAlloc->beginBlock();
+
+	//	int memsize = 16;
+//		unsigned char* stackMemory = stackAlloc->allocate(memsize);
+
+		
+		//todo: use stack allocator for this temp memory
+//		int minReservation = numManifolds*2;
+
+		//m_tmpSolverBodyPool.reserve(minReservation);
+
+		//don't convert all bodies, only the one we need so solver the constraints
+/*
+		{
+			for (int i=0;i<numBodies;i++)
+			{
+				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
+				if (rb && 	(rb->getIslandTag() >= 0))
+				{
+					btAssert(rb->getCompanionId() < 0);
+					int solverBodyId = m_tmpSolverBodyPool.size();
+					btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+					initSolverBody(&solverBody,rb);
+					rb->setCompanionId(solverBodyId);
+				} 
+			}
+		}
+*/
+		
+		//m_tmpSolverConstraintPool.reserve(minReservation);
+		//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
+
+		{
+			int i;
+
+			for (i=0;i<numManifolds;i++)
+			{
+				manifold = manifoldPtr[i];
+				colObj0 = (btCollisionObject*)manifold->getBody0();
+				colObj1 = (btCollisionObject*)manifold->getBody1();
+			
+				int solverBodyIdA=-1;
+				int solverBodyIdB=-1;
+
+				if (manifold->getNumContacts())
+				{
+
+					
+
+					if (colObj0->getIslandTag() >= 0)
+					{
+						if (colObj0->getCompanionId() >= 0)
+						{
+							//body has already been converted
+							solverBodyIdA = colObj0->getCompanionId();
+						} else
+						{
+							solverBodyIdA = m_tmpSolverBodyPool.size();
+							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,colObj0);
+							colObj0->setCompanionId(solverBodyIdA);
+						}
+					} else
+					{
+						//create a static body
+						solverBodyIdA = m_tmpSolverBodyPool.size();
+						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,colObj0);
+					}
+
+					if (colObj1->getIslandTag() >= 0)
+					{
+						if (colObj1->getCompanionId() >= 0)
+						{
+							solverBodyIdB = colObj1->getCompanionId();
+						} else
+						{
+							solverBodyIdB = m_tmpSolverBodyPool.size();
+							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,colObj1);
+							colObj1->setCompanionId(solverBodyIdB);
+						}
+					} else
+					{
+						//create a static body
+						solverBodyIdB = m_tmpSolverBodyPool.size();
+						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,colObj1);
+					}
+				}
+
+				btVector3 rel_pos1;
+				btVector3 rel_pos2;
+				btScalar relaxation;
+
+				for (int j=0;j<manifold->getNumContacts();j++)
+				{
+					
+					btManifoldPoint& cp = manifold->getContactPoint(j);
+					
+					if (cp.getDistance() <= btScalar(0.))
+					{
+						
+						const btVector3& pos1 = cp.getPositionWorldOnA();
+						const btVector3& pos2 = cp.getPositionWorldOnB();
+
+						 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+						 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+						
+						relaxation = 1.f;
+						btScalar rel_vel;
+						btVector3 vel;
+
+						int frictionIndex = m_tmpSolverConstraintPool.size();
+
+						{
+							btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
+							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+
+							solverConstraint.m_solverBodyIdA = solverBodyIdA;
+							solverConstraint.m_solverBodyIdB = solverBodyIdB;
+							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
+
+							solverConstraint.m_originalContactPoint = &cp;
+
+							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+							solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
+							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+							solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
+							{
+#ifdef COMPUTE_IMPULSE_DENOM
+								btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+								btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else							
+								btVector3 vec;
+								btScalar denom0 = 0.f;
+								btScalar denom1 = 0.f;
+								if (rb0)
+								{
+									vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+									denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+								}
+								if (rb1)
+								{
+									vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+									denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+								}
+#endif //COMPUTE_IMPULSE_DENOM		
+								
+								btScalar denom = relaxation/(denom0+denom1);
+								solverConstraint.m_jacDiagABInv = denom;
+							}
+
+							solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+							solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
+
+
+							btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+							btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+				
+							vel  = vel1 - vel2;
+							
+							rel_vel = cp.m_normalWorldOnB.dot(vel);
+							
+							solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
+							//solverConstraint.m_penetration = cp.getDistance();
+
+							solverConstraint.m_friction = cp.m_combinedFriction;
+
+							
+							if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+							{
+								solverConstraint.m_restitution = 0.f;
+							} else
+							{
+								solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+								if (solverConstraint.m_restitution <= btScalar(0.))
+								{
+									solverConstraint.m_restitution = 0.f;
+								};
+							}
+
+							
+							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
+
+							
+
+							if (solverConstraint.m_restitution > penVel)
+							{
+								solverConstraint.m_penetration = btScalar(0.);
+							}
+							 
+							
+							
+							///warm starting (or zero if disabled)
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+							} else
+							{
+								solverConstraint.m_appliedImpulse = 0.f;
+							}
+
+							solverConstraint.m_appliedPushImpulse = 0.f;
+							
+							solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
+							if (!cp.m_lateralFrictionInitialized)
+							{
+								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+								btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
+								{
+									cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+									cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+									cp.m_lateralFrictionDir2.normalize();//??
+									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								} else
+								{
+									//re-calculate friction direction every frame, todo: check if this is really needed
+									
+									btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								}
+								cp.m_lateralFrictionInitialized = true;
+								
+							} else
+							{
+								addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
+
+							{
+								btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
+								if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+								{
+									frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+									if (rb0)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+									if (rb1)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+								} else
+								{
+									frictionConstraint1.m_appliedImpulse = 0.f;
+								}
+							}
+							{
+								btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+								if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+								{
+									frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+									if (rb0)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+									if (rb1)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+								} else
+								{
+									frictionConstraint2.m_appliedImpulse = 0.f;
+								}
+							}
+						}
+
+
+					}
+				}
+			}
+		}
+	}
+	
+	btContactSolverInfo info = infoGlobal;
+
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
+	
+	
+
+	int numConstraintPool = m_tmpSolverConstraintPool.size();
+	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	m_orderTmpConstraintPool.resize(numConstraintPool);
+	m_orderFrictionConstraintPool.resize(numFrictionPool);
+	{
+		int i;
+		for (i=0;i<numConstraintPool;i++)
+		{
+			m_orderTmpConstraintPool[i] = i;
+		}
+		for (i=0;i<numFrictionPool;i++)
+		{
+			m_orderFrictionConstraintPool[i] = i;
+		}
+	}
+
+
+
+	return 0.f;
+
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
+	BT_PROFILE("solveGroupCacheFriendlyIterations");
+	int numConstraintPool = m_tmpSolverConstraintPool.size();
+	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	//should traverse the contacts random order...
+	int iteration;
+	{
+		for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+		{			
+
+			int j;
+			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
+				if ((iteration & 7) == 0) {
+					for (j=0; j<numConstraintPool; ++j) {
+						int tmp = m_orderTmpConstraintPool[j];
+						int swapi = btRandInt2(j+1);
+						m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+						m_orderTmpConstraintPool[swapi] = tmp;
+					}
+
+					for (j=0; j<numFrictionPool; ++j) {
+						int tmp = m_orderFrictionConstraintPool[j];
+						int swapi = btRandInt2(j+1);
+						m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+						m_orderFrictionConstraintPool[swapi] = tmp;
+					}
+				}
+			}
+
+			for (j=0;j<numConstraints;j++)
+			{
+				btTypedConstraint* constraint = constraints[j];
+				///todo: use solver bodies, so we don't need to copy from/to btRigidBody
+
+				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
+				{
+					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
+				}
+				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
+				{
+					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
+				}
+
+				constraint->solveConstraint(infoGlobal.m_timeStep);
+
+				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
+				{
+					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
+				}
+				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
+				{
+					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
+				}
+
+			}
+
+			{
+				int numPoolConstraints = m_tmpSolverConstraintPool.size();
+				for (j=0;j<numPoolConstraints;j++)
+				{
+					
+					const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+					resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+						m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+				}
+			}
+
+			{
+				 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
+				
+				 for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
+								m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;			
+
+						resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
+							totalImpulse);
+				}
+			}
+			
+
+
+		}
+	
+		if (infoGlobal.m_splitImpulse)
+		{
+			
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverConstraintPool.size();
+					int j;
+					for (j=0;j<numPoolConstraints;j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+					}
+				}
+			}
+
+		}
+
+	}
+
+	return 0.f;
+}
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+	int i;
+
+	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+
+	int numPoolConstraints = m_tmpSolverConstraintPool.size();
+	int j;
+	for (j=0;j<numPoolConstraints;j++)
+	{
+		
+		const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+		btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+		pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+
+		//do a callback here?
+
+	}
+
+	if (infoGlobal.m_splitImpulse)
+	{		
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+		{
+			m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+		}
+	} else
+	{
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+        {
+                m_tmpSolverBodyPool[i].writebackVelocity();
+        }
+	}
+
+//	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
+
+/*
+	printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
+	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
+	printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
+
+	
+	printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
+	printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
+	printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
+*/
+
+	m_tmpSolverBodyPool.resize(0);
+	m_tmpSolverConstraintPool.resize(0);
+	m_tmpSolverFrictionConstraintPool.resize(0);
+
+
+	return 0.f;
+}
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+	BT_PROFILE("solveGroup");
+	if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
+	{
+		//you need to provide at least some bodies
+		//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
+		btAssert(bodies);
+		btAssert(numBodies);
+		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+	}
+
+	
+
+	btContactSolverInfo info = infoGlobal;
+
+	int numiter = infoGlobal.m_numIterations;
+
+	int totalPoints = 0;
+
+
+	{
+		short j;
+		for (j=0;j<numManifolds;j++)
+		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
+
+			for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
+			{
+				gOrder[totalPoints].m_manifoldIndex = j;
+				gOrder[totalPoints].m_pointIndex = p;
+				totalPoints++;
+			}
+		}
+	}
+
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
+	
+
+	//should traverse the contacts random order...
+	int iteration;
+
+	{
+		for ( iteration = 0;iteration<numiter;iteration++)
+		{
+			int j;
+			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
+				if ((iteration & 7) == 0) {
+					for (j=0; j<totalPoints; ++j) {
+						btOrderIndex tmp = gOrder[j];
+						int swapi = btRandInt2(j+1);
+						gOrder[j] = gOrder[swapi];
+						gOrder[swapi] = tmp;
+					}
+				}
+			}
+
+			for (j=0;j<numConstraints;j++)
+			{
+				btTypedConstraint* constraint = constraints[j];
+				constraint->solveConstraint(info.m_timeStep);
+			}
+
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+		
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+			
+		}
+	}
+		
+
+
+
+	return btScalar(0.);
+}
+
+
+
+
+
+
+
+void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
+{
+
+	(void)debugDrawer;
+
+	btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
+	btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
+
+
+	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
+	{
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
+		int numpoints = manifoldPtr->getNumContacts();
+
+		gTotalContactPoints += numpoints;
+
+		
+		for (int i=0;i<numpoints ;i++)
+		{
+			btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
+			if (cp.getDistance() <= btScalar(0.))
+			{
+				const btVector3& pos1 = cp.getPositionWorldOnA();
+				const btVector3& pos2 = cp.getPositionWorldOnB();
+
+				btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
+				btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
+				
+
+				//this jacobian entry is re-used for all iterations
+				btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
+					body1->getCenterOfMassTransform().getBasis().transpose(),
+					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
+					body1->getInvInertiaDiagLocal(),body1->getInvMass());
+
+				
+				btScalar jacDiagAB = jac.getDiagonal();
+
+				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+				if (cpd)
+				{
+					//might be invalid
+					cpd->m_persistentLifeTime++;
+					if (cpd->m_persistentLifeTime != cp.getLifeTime())
+					{
+						//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
+						new (cpd) btConstraintPersistentData;
+						cpd->m_persistentLifeTime = cp.getLifeTime();
+
+					} else
+					{
+						//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
+						
+					}
+				} else
+				{
+						
+					//todo: should this be in a pool?
+					void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
+					cpd = new (mem)btConstraintPersistentData;
+					assert(cpd);
+
+					totalCpd ++;
+					//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
+					cp.m_userPersistentData = cpd;
+					cpd->m_persistentLifeTime = cp.getLifeTime();
+					//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
+					
+				}
+				assert(cpd);
+
+				cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
+
+				//Dependent on Rigidbody A and B types, fetch the contact/friction response func
+				//perhaps do a similar thing for friction/restutution combiner funcs...
+				
+				cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
+				cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
+				
+				btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
+				btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
+				btVector3 vel = vel1 - vel2;
+				btScalar rel_vel;
+				rel_vel = cp.m_normalWorldOnB.dot(vel);
+				
+				btScalar combinedRestitution = cp.m_combinedRestitution;
+				
+				cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
+				cpd->m_friction = cp.m_combinedFriction;
+				if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
+				{
+					cpd->m_restitution = 0.f;
+				} else
+				{
+					cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
+					if (cpd->m_restitution <= btScalar(0.))
+					{
+						cpd->m_restitution = btScalar(0.0);
+					};
+				}
+				
+				//restitution and penetration work in same direction so
+				//rel_vel 
+				
+				btScalar penVel = -cpd->m_penetration/info.m_timeStep;
+
+				if (cpd->m_restitution > penVel)
+				{
+					cpd->m_penetration = btScalar(0.);
+				} 				
+				
+
+				btScalar relaxation = info.m_damping;
+				if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					cpd->m_appliedImpulse *= relaxation;
+				} else
+				{
+					cpd->m_appliedImpulse =btScalar(0.);
+				}
+	
+				//for friction
+				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
+				
+				//re-calculate friction direction every frame, todo: check if this is really needed
+				btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
+
+
+#define NO_FRICTION_WARMSTART 1
+
+	#ifdef NO_FRICTION_WARMSTART
+				cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
+				cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
+	#endif //NO_FRICTION_WARMSTART
+				btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
+				btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
+				btScalar denom = relaxation/(denom0+denom1);
+				cpd->m_jacDiagABInvTangent0 = denom;
+
+
+				denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
+				denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
+				denom = relaxation/(denom0+denom1);
+				cpd->m_jacDiagABInvTangent1 = denom;
+
+				btVector3 totalImpulse = 
+	#ifndef NO_FRICTION_WARMSTART
+					cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
+					cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
+	#endif //NO_FRICTION_WARMSTART
+					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
+
+
+
+				///
+				{
+				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+				cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
+				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+				cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				
+				///
+
+
+
+				//apply previous frames impulse on both bodies
+				body0->applyImpulse(totalImpulse, rel_pos1);
+				body1->applyImpulse(-totalImpulse, rel_pos2);
+			}
+			
+		}
+	}
+}
+
+
+btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+{
+	btScalar maxImpulse = btScalar(0.);
+
+	{
+
+		
+		{
+			if (cp.getDistance() <= btScalar(0.))
+			{
+
+				
+
+				{
+
+					//btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+					btScalar impulse = resolveSingleCollisionCombined(
+						*body0,*body1,
+						cp,
+						info);
+
+					if (maxImpulse < impulse)
+						maxImpulse  = impulse;
+
+				}
+			}
+		}
+	}
+	return maxImpulse;
+}
+
+
+
+btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+{
+
+	btScalar maxImpulse = btScalar(0.);
+
+	{
+
+		
+		{
+			if (cp.getDistance() <= btScalar(0.))
+			{
+
+				
+
+				{
+
+					btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+					btScalar impulse = cpd->m_contactSolverFunc(
+						*body0,*body1,
+						cp,
+						info);
+
+					if (maxImpulse < impulse)
+						maxImpulse  = impulse;
+
+				}
+			}
+		}
+	}
+	return maxImpulse;
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+{
+
+	(void)debugDrawer;
+	(void)iter;
+
+
+	{
+
+		
+		{
+			
+			if (cp.getDistance() <= btScalar(0.))
+			{
+
+				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+				cpd->m_frictionSolverFunc(
+					*body0,*body1,
+					cp,
+					info);
+
+				
+			}
+		}
+
+	
+	}
+	return btScalar(0.);
+}
+
+
+void	btSequentialImpulseConstraintSolver::reset()
+{
+	m_btSeed2 = 0;
+}
+
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 1963)
@@ -0,0 +1,109 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+
+#include "btConstraintSolver.h"
+class btIDebugDraw;
+#include "btContactConstraint.h"
+#include "btSolverBody.h"
+#include "btSolverConstraint.h"
+
+
+/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
+/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
+/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
+/// Applies impulses for combined restitution and penetration recovery and to simulate friction
+class btSequentialImpulseConstraintSolver : public btConstraintSolver
+{
+
+	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
+	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverConstraintPool;
+	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverFrictionConstraintPool;
+	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
+	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
+
+
+protected:
+	btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+	btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
+	void	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
+
+	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+
+	
+	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+	unsigned long	m_btSeed2;
+
+public:
+
+	
+	btSequentialImpulseConstraintSolver();
+
+	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
+	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+	void	setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
+	{
+		m_contactDispatch[type0][type1] = func;
+	}
+	
+	///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
+	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+	void	SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
+	{
+		m_frictionDispatch[type0][type1] = func;
+	}
+
+	virtual ~btSequentialImpulseConstraintSolver();
+	
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
+	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset();
+
+	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+
+
+	
+	unsigned long btRand2();
+
+	int btRandInt2 (int n);
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_btSeed2 = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_btSeed2;
+	}
+
+};
+
+#ifndef BT_PREFER_SIMD
+typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
+#endif
+
+
+#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 1963)
@@ -0,0 +1,415 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+*/
+
+//-----------------------------------------------------------------------------
+
+#include "btSliderConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::initParams()
+{
+    m_lowerLinLimit = btScalar(1.0);
+    m_upperLinLimit = btScalar(-1.0);
+    m_lowerAngLimit = btScalar(0.);
+    m_upperAngLimit = btScalar(0.);
+	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingDirLin = btScalar(0.);
+	m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingDirAng = btScalar(0.);
+	m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+	m_poweredLinMotor = false;
+    m_targetLinMotorVelocity = btScalar(0.);
+    m_maxLinMotorForce = btScalar(0.);
+	m_accumulatedLinMotorImpulse = btScalar(0.0);
+
+	m_poweredAngMotor = false;
+    m_targetAngMotorVelocity = btScalar(0.);
+    m_maxAngMotorForce = btScalar(0.);
+	m_accumulatedAngMotorImpulse = btScalar(0.0);
+
+} // btSliderConstraint::initParams()
+
+//-----------------------------------------------------------------------------
+
+btSliderConstraint::btSliderConstraint()
+        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
+		m_useLinearReferenceFrameA(true)
+{
+	initParams();
+} // btSliderConstraint::btSliderConstraint()
+
+//-----------------------------------------------------------------------------
+
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+	initParams();
+} // btSliderConstraint::btSliderConstraint()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::buildJacobian()
+{
+	if(m_useLinearReferenceFrameA)
+	{
+		buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+	}
+	else
+	{
+		buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
+	}
+} // btSliderConstraint::buildJacobian()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+{
+	//calculate transforms
+    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
+    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+	m_realPivotAInW = m_calculatedTransformA.getOrigin();
+	m_realPivotBInW = m_calculatedTransformB.getOrigin();
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+	m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
+	m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		new (&m_jacLin[i]) btJacobianEntry(
+			rbA.getCenterOfMassTransform().getBasis().transpose(),
+			rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_relPosA,
+			m_relPosB,
+			normalWorld,
+			rbA.getInvInertiaDiagLocal(),
+			rbA.getInvMass(),
+			rbB.getInvInertiaDiagLocal(),
+			rbB.getInvMass()
+			);
+		m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
+		m_depth[i] = m_delta.dot(normalWorld);
+    }
+	testLinLimits();
+    // angular part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		new (&m_jacAng[i])	btJacobianEntry(
+			normalWorld,
+            rbA.getCenterOfMassTransform().getBasis().transpose(),
+            rbB.getCenterOfMassTransform().getBasis().transpose(),
+            rbA.getInvInertiaDiagLocal(),
+            rbB.getInvInertiaDiagLocal()
+			);
+	}
+	testAngLimits();
+	btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+	m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
+	// clear accumulator for motors
+	m_accumulatedLinMotorImpulse = btScalar(0.0);
+	m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::buildJacobianInt()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraint(btScalar timeStep)
+{
+    m_timeStep = timeStep;
+	if(m_useLinearReferenceFrameA)
+	{
+		solveConstraintInt(m_rbA, m_rbB);
+	}
+	else
+	{
+		solveConstraintInt(m_rbB, m_rbA);
+	}
+} // btSliderConstraint::solveConstraint()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+{
+    int i;
+    // linear
+    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
+    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+    btVector3 vel = velA - velB;
+	for(i = 0; i < 3; i++)
+    {
+		const btVector3& normal = m_jacLin[i].m_linearJointAxis;
+		btScalar rel_vel = normal.dot(vel);
+		// calculate positional error
+		btScalar depth = m_depth[i];
+		// get parameters
+		btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+		btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+		btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+		// calcutate and apply impulse
+		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
+		btVector3 impulse_vector = normal * normalImpulse;
+		rbA.applyImpulse( impulse_vector, m_relPosA);
+		rbB.applyImpulse(-impulse_vector, m_relPosB);
+		if(m_poweredLinMotor && (!i))
+		{ // apply linear motor
+			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
+			{
+				btScalar desiredMotorVel = m_targetLinMotorVelocity;
+				btScalar motor_relvel = desiredMotorVel + rel_vel;
+				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+				// clamp accumulated impulse
+				btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
+				if(new_acc  > m_maxLinMotorForce)
+				{
+					new_acc = m_maxLinMotorForce;
+				}
+				btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
+				if(normalImpulse < btScalar(0.0))
+				{
+					normalImpulse = -del;
+				}
+				else
+				{
+					normalImpulse = del;
+				}
+				m_accumulatedLinMotorImpulse = new_acc;
+				// apply clamped impulse
+				impulse_vector = normal * normalImpulse;
+				rbA.applyImpulse( impulse_vector, m_relPosA);
+				rbB.applyImpulse(-impulse_vector, m_relPosB);
+			}
+		}
+    }
+	// angular 
+	// get axes in world space
+	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
+	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
+
+	const btVector3& angVelA = rbA.getAngularVelocity();
+	const btVector3& angVelB = rbB.getAngularVelocity();
+
+	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+
+	btVector3 angAorthog = angVelA - angVelAroundAxisA;
+	btVector3 angBorthog = angVelB - angVelAroundAxisB;
+	btVector3 velrelOrthog = angAorthog-angBorthog;
+	//solve orthogonal angular velocity correction
+	btScalar len = velrelOrthog.length();
+	if (len > btScalar(0.00001))
+	{
+		btVector3 normal = velrelOrthog.normalized();
+		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
+		velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+	}
+	//solve angular positional correction
+	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+	btScalar len2 = angularError.length();
+	if (len2>btScalar(0.00001))
+	{
+		btVector3 normal2 = angularError.normalized();
+		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
+		angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+	}
+	// apply impulse
+	rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+	rbB.applyTorqueImpulse(velrelOrthog-angularError);
+	btScalar impulseMag;
+	//solve angular limits
+	if(m_solveAngLim)
+	{
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
+		impulseMag *= m_kAngle * m_softnessLimAng;
+	}
+	else
+	{
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
+		impulseMag *= m_kAngle * m_softnessDirAng;
+	}
+	btVector3 impulse = axisA * impulseMag;
+	rbA.applyTorqueImpulse(impulse);
+	rbB.applyTorqueImpulse(-impulse);
+	//apply angular motor
+	if(m_poweredAngMotor) 
+	{
+		if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+		{
+			btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+			btScalar projRelVel = velrel.dot(axisA);
+
+			btScalar desiredMotorVel = m_targetAngMotorVelocity;
+			btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+			btScalar angImpulse = m_kAngle * motor_relvel;
+			// clamp accumulated impulse
+			btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
+			if(new_acc  > m_maxAngMotorForce)
+			{
+				new_acc = m_maxAngMotorForce;
+			}
+			btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
+			if(angImpulse < btScalar(0.0))
+			{
+				angImpulse = -del;
+			}
+			else
+			{
+				angImpulse = del;
+			}
+			m_accumulatedAngMotorImpulse = new_acc;
+			// apply clamped impulse
+			btVector3 motorImp = angImpulse * axisA;
+			m_rbA.applyTorqueImpulse(motorImp);
+			m_rbB.applyTorqueImpulse(-motorImp);
+		}
+	}
+} // btSliderConstraint::solveConstraint()
+
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::calculateTransforms(void){
+	if(m_useLinearReferenceFrameA)
+	{
+		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+		m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	}
+	else
+	{
+		m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
+		m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	}
+	m_realPivotAInW = m_calculatedTransformA.getOrigin();
+	m_realPivotBInW = m_calculatedTransformB.getOrigin();
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		m_depth[i] = m_delta.dot(normalWorld);
+    }
+} // btSliderConstraint::calculateTransforms()
+ 
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::testLinLimits(void)
+{
+	m_solveLinLim = false;
+	m_linPos = m_depth[0];
+	if(m_lowerLinLimit <= m_upperLinLimit)
+	{
+		if(m_depth[0] > m_upperLinLimit)
+		{
+			m_depth[0] -= m_upperLinLimit;
+			m_solveLinLim = true;
+		}
+		else if(m_depth[0] < m_lowerLinLimit)
+		{
+			m_depth[0] -= m_lowerLinLimit;
+			m_solveLinLim = true;
+		}
+		else
+		{
+			m_depth[0] = btScalar(0.);
+		}
+	}
+	else
+	{
+		m_depth[0] = btScalar(0.);
+	}
+} // btSliderConstraint::testLinLimits()
+
+//-----------------------------------------------------------------------------
+ 
+
+void btSliderConstraint::testAngLimits(void)
+{
+	m_angDepth = btScalar(0.);
+	m_solveAngLim = false;
+	if(m_lowerAngLimit <= m_upperAngLimit)
+	{
+		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		if(rot < m_lowerAngLimit)
+		{
+			m_angDepth = rot - m_lowerAngLimit;
+			m_solveAngLim = true;
+		} 
+		else if(rot > m_upperAngLimit)
+		{
+			m_angDepth = rot - m_upperAngLimit;
+			m_solveAngLim = true;
+		}
+	}
+} // btSliderConstraint::testAngLimits()
+
+	
+//-----------------------------------------------------------------------------
+
+
+
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+	btVector3 ancorInA;
+	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+	ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+	return ancorInA;
+} // btSliderConstraint::getAncorInA()
+
+//-----------------------------------------------------------------------------
+
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+	btVector3 ancorInB;
+	ancorInB = m_frameInB.getOrigin();
+	return ancorInB;
+} // btSliderConstraint::getAncorInB();
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 1963)
@@ -0,0 +1,218 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+
+TODO:
+ - add clamping od accumulated impulse to improve stability
+ - add conversion for ODE constraint solver
+*/
+
+#ifndef SLIDER_CONSTRAINT_H
+#define SLIDER_CONSTRAINT_H
+
+//-----------------------------------------------------------------------------
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+//-----------------------------------------------------------------------------
+
+class btRigidBody;
+
+//-----------------------------------------------------------------------------
+
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))
+
+//-----------------------------------------------------------------------------
+
+class btSliderConstraint : public btTypedConstraint
+{
+protected:
+	btTransform	m_frameInA;
+    btTransform	m_frameInB;
+	// use frameA fo define limits, if true
+	bool m_useLinearReferenceFrameA;
+	// linear limits
+	btScalar m_lowerLinLimit;
+	btScalar m_upperLinLimit;
+	// angular limits
+	btScalar m_lowerAngLimit;
+	btScalar m_upperAngLimit;
+	// softness, restitution and damping for different cases
+	// DirLin - moving inside linear limits
+	// LimLin - hitting linear limit
+	// DirAng - moving inside angular limits
+	// LimAng - hitting angular limit
+	// OrthoLin, OrthoAng - against constraint axis
+	btScalar m_softnessDirLin;
+	btScalar m_restitutionDirLin;
+	btScalar m_dampingDirLin;
+	btScalar m_softnessDirAng;
+	btScalar m_restitutionDirAng;
+	btScalar m_dampingDirAng;
+	btScalar m_softnessLimLin;
+	btScalar m_restitutionLimLin;
+	btScalar m_dampingLimLin;
+	btScalar m_softnessLimAng;
+	btScalar m_restitutionLimAng;
+	btScalar m_dampingLimAng;
+	btScalar m_softnessOrthoLin;
+	btScalar m_restitutionOrthoLin;
+	btScalar m_dampingOrthoLin;
+	btScalar m_softnessOrthoAng;
+	btScalar m_restitutionOrthoAng;
+	btScalar m_dampingOrthoAng;
+	
+	// for interlal use
+	bool m_solveLinLim;
+	bool m_solveAngLim;
+
+	btJacobianEntry	m_jacLin[3];
+	btScalar		m_jacLinDiagABInv[3];
+
+    btJacobianEntry	m_jacAng[3];
+
+	btScalar m_timeStep;
+    btTransform m_calculatedTransformA;
+    btTransform m_calculatedTransformB;
+
+	btVector3 m_sliderAxis;
+	btVector3 m_realPivotAInW;
+	btVector3 m_realPivotBInW;
+	btVector3 m_projPivotInW;
+	btVector3 m_delta;
+	btVector3 m_depth;
+	btVector3 m_relPosA;
+	btVector3 m_relPosB;
+
+	btScalar m_linPos;
+
+	btScalar m_angDepth;
+	btScalar m_kAngle;
+
+	bool	 m_poweredLinMotor;
+    btScalar m_targetLinMotorVelocity;
+    btScalar m_maxLinMotorForce;
+    btScalar m_accumulatedLinMotorImpulse;
+	
+	bool	 m_poweredAngMotor;
+    btScalar m_targetAngMotorVelocity;
+    btScalar m_maxAngMotorForce;
+    btScalar m_accumulatedAngMotorImpulse;
+
+	//------------------------    
+	void initParams();
+public:
+	// constructors
+    btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btSliderConstraint();
+	// overrides
+    virtual void	buildJacobian();
+    virtual	void	solveConstraint(btScalar	timeStep);
+	// access
+    const btRigidBody& getRigidBodyA() const { return m_rbA; }
+    const btRigidBody& getRigidBodyB() const { return m_rbB; }
+    const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+    const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+    const btTransform & getFrameOffsetA() const { return m_frameInA; }
+    const btTransform & getFrameOffsetB() const { return m_frameInB; }
+    btTransform & getFrameOffsetA() { return m_frameInA; }
+    btTransform & getFrameOffsetB() { return m_frameInB; }
+    btScalar getLowerLinLimit() { return m_lowerLinLimit; }
+    void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
+    btScalar getUpperLinLimit() { return m_upperLinLimit; }
+    void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
+    btScalar getLowerAngLimit() { return m_lowerAngLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    btScalar getUpperAngLimit() { return m_upperAngLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
+	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
+	btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
+	btScalar getDampingDirLin() { return m_dampingDirLin ; }
+	btScalar getSoftnessDirAng() { return m_softnessDirAng; }
+	btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
+	btScalar getDampingDirAng() { return m_dampingDirAng; }
+	btScalar getSoftnessLimLin() { return m_softnessLimLin; }
+	btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
+	btScalar getDampingLimLin() { return m_dampingLimLin; }
+	btScalar getSoftnessLimAng() { return m_softnessLimAng; }
+	btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
+	btScalar getDampingLimAng() { return m_dampingLimAng; }
+	btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
+	btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
+	btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
+	btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
+	btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
+	btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
+	void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
+	void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
+	void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
+	void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
+	void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
+	void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
+	void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
+	void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
+	void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
+	void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
+	void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
+	void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
+	void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
+	void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
+	void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
+	void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
+	void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
+	void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
+	void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
+	bool getPoweredLinMotor() { return m_poweredLinMotor; }
+	void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
+	btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
+	void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
+	btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
+	void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
+	bool getPoweredAngMotor() { return m_poweredAngMotor; }
+	void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
+	btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
+	void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
+	btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
+	btScalar getLinearPos() { return m_linPos; }
+
+	// access for ODE solver
+	bool getSolveLinLimit() { return m_solveLinLim; }
+	btScalar getLinDepth() { return m_depth[0]; }
+	bool getSolveAngLimit() { return m_solveAngLim; }
+	btScalar getAngDepth() { return m_angDepth; }
+	// internal
+    void	buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
+    void	solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+	// shared code used by ODE solver
+	void	calculateTransforms(void);
+	void	testLinLimits(void);
+	void	testAngLimits(void);
+	// access for PE Solver
+	btVector3 getAncorInA(void);
+	btVector3 getAncorInB(void);
+};
+
+//-----------------------------------------------------------------------------
+
+#endif //SLIDER_CONSTRAINT_H
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp	(revision 1963)
@@ -0,0 +1,255 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btSolve2LinearConstraint.h"
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+
+
+void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
+												   btRigidBody* body1,
+		btRigidBody* body2,
+
+						const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1)
+{
+	(void)linvelA;
+	(void)linvelB;
+	(void)angvelB;
+	(void)angvelA;
+
+
+
+	imp0 = btScalar(0.);
+	imp1 = btScalar(0.);
+
+	btScalar len = btFabs(normalA.length()) - btScalar(1.);
+	if (btFabs(len) >= SIMD_EPSILON)
+		return;
+
+	btAssert(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+	btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
+
+
+	// calculate rhs (or error) terms
+	const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
+	const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
+
+
+	// dC/dv * dv = -C
+	
+	// jacobian * impulse = -error
+	//
+
+	//impulse = jacobianInverse * -error
+
+	// inverting 2x2 symmetric system (offdiagonal are equal!)
+	// 
+
+
+	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+	
+	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	//[a b]								  [d -c]
+	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+	//[jA nD] * [imp0] = [dv0]
+	//[nD jB]   [imp1]   [dv1]
+
+}
+
+
+
+void btSolve2LinearConstraint::resolveBilateralPairConstraint(
+						btRigidBody* body1,
+						btRigidBody* body2,
+						const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1)
+{
+
+	(void)linvelA;
+	(void)linvelB;
+	(void)angvelA;
+	(void)angvelB;
+
+
+
+	imp0 = btScalar(0.);
+	imp1 = btScalar(0.);
+
+	btScalar len = btFabs(normalA.length()) - btScalar(1.);
+	if (btFabs(len) >= SIMD_EPSILON)
+		return;
+
+	btAssert(len < SIMD_EPSILON);
+
+
+	//this jacobian entry could be re-used for all iterations
+	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
+		invInertiaBDiag,invMassB);
+	
+	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
+
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+
+	// calculate rhs (or error) terms
+	const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
+	const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
+
+	// dC/dv * dv = -C
+	
+	// jacobian * impulse = -error
+	//
+
+	//impulse = jacobianInverse * -error
+
+	// inverting 2x2 symmetric system (offdiagonal are equal!)
+	// 
+
+
+	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
+	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
+	
+	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+
+	//[a b]								  [d -c]
+	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
+
+	//[jA nD] * [imp0] = [dv0]
+	//[nD jB]   [imp1]   [dv1]
+
+	if ( imp0 > btScalar(0.0))
+	{
+		if ( imp1 > btScalar(0.0) )
+		{
+			//both positive
+		}
+		else
+		{
+			imp1 = btScalar(0.);
+
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > btScalar(0.0) )
+			{
+			} else
+			{
+				imp0 = btScalar(0.);
+			}
+		}
+	}
+	else
+	{
+		imp0 = btScalar(0.);
+
+		imp1 = dv1 / jacB.getDiagonal();
+		if ( imp1 <= btScalar(0.0) )
+		{
+			imp1 = btScalar(0.);
+			// now imp0>0 imp1<0
+			imp0 = dv0 / jacA.getDiagonal();
+			if ( imp0 > btScalar(0.0) )
+			{
+			} else
+			{
+				imp0 = btScalar(0.);
+			}
+		} else
+		{
+		}
+	}
+}
+
+
+/*
+void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
+											const btScalar invMassA,
+											const btVector3& linvelA,const btVector3& angvelA,
+											const btVector3& rel_posA1,
+											const btMatrix3x3& invInertiaBWS,
+											const btScalar invMassB,
+											const btVector3& linvelB,const btVector3& angvelB,
+											const btVector3& rel_posA2,
+
+											btScalar depthA, const btVector3& normalA, 
+											const btVector3& rel_posB1,const btVector3& rel_posB2,
+											btScalar depthB, const btVector3& normalB, 
+											btScalar& imp0,btScalar& imp1)
+{
+
+}
+*/
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h	(revision 1963)
@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOLVE_2LINEAR_CONSTRAINT_H
+#define SOLVE_2LINEAR_CONSTRAINT_H
+
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btVector3.h"
+
+
+class btRigidBody;
+
+
+
+/// constraint class used for lateral tyre friction.
+class	btSolve2LinearConstraint
+{
+	btScalar	m_tau;
+	btScalar	m_damping;
+
+public:
+
+	btSolve2LinearConstraint(btScalar tau,btScalar damping)
+	{
+		m_tau = tau;
+		m_damping = damping;
+	}
+	//
+	// solve unilateral constraint (equality, direct method)
+	//
+	void resolveUnilateralPairConstraint(		
+														   btRigidBody* body0,
+		btRigidBody* body1,
+
+		const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+
+	//
+	// solving 2x2 lcp problem (inequality, direct solution )
+	//
+	void resolveBilateralPairConstraint(
+			btRigidBody* body0,
+						btRigidBody* body1,
+		const btMatrix3x3& world2A,
+						const btMatrix3x3& world2B,
+						
+						const btVector3& invInertiaADiag,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btVector3& invInertiaBDiag,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+/*
+	void resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
+						const btScalar invMassA,
+						const btVector3& linvelA,const btVector3& angvelA,
+						const btVector3& rel_posA1,
+						const btMatrix3x3& invInertiaBWS,
+						const btScalar invMassB,
+						const btVector3& linvelB,const btVector3& angvelB,
+						const btVector3& rel_posA2,
+
+					  btScalar depthA, const btVector3& normalA, 
+					  const btVector3& rel_posB1,const btVector3& rel_posB2,
+					  btScalar depthB, const btVector3& normalB, 
+					  btScalar& imp0,btScalar& imp1);
+
+*/
+
+};
+
+#endif //SOLVE_2LINEAR_CONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 1963)
@@ -0,0 +1,113 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_BODY_H
+#define BT_SOLVER_BODY_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	btVector3		m_angularVelocity;
+	float			m_angularFactor;
+	float			m_invMass;
+	float			m_friction;
+	btRigidBody*	m_originalBody;
+	btVector3		m_linearVelocity;
+	btVector3		m_centerOfMassPosition;
+	
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+	
+	
+	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+	
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_invMass)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+	
+	void	writebackVelocity()
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+	
+
+	void	writebackVelocity(btScalar timeStep)
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//correct the position/orientation based on push/turn recovery
+			btTransform newTransform;
+			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+			m_originalBody->setWorldTransform(newTransform);
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+	void	readVelocity()
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity = m_originalBody->getLinearVelocity();
+			m_angularVelocity = m_originalBody->getAngularVelocity();
+		}
+	}
+
+	
+
+
+};
+
+#endif //BT_SOLVER_BODY_H
+
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 1963)
@@ -0,0 +1,72 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_CONSTRAINT_H
+#define BT_SOLVER_CONSTRAINT_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btVector3	m_relpos1CrossNormal;
+	btVector3	m_contactNormal;
+
+	btVector3	m_relpos2CrossNormal;
+	btVector3	m_angularComponentA;
+
+	btVector3	m_angularComponentB;
+
+	mutable btScalar	m_appliedPushImpulse;
+	
+	mutable btScalar	m_appliedImpulse;
+	int			m_solverBodyIdA;
+	int			m_solverBodyIdB;
+	
+	btScalar	m_friction;
+	btScalar	m_restitution;
+	btScalar	m_jacDiagABInv;
+	btScalar	m_penetration;
+	
+
+	
+	int			m_constraintType;
+	int			m_frictionIndex;
+	void*		m_originalContactPoint;
+	int			m_unusedPadding[1];
+
+
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+
+
+
+
+
+#endif //BT_SOLVER_CONSTRAINT_H
+
+
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 1963)
@@ -0,0 +1,56 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+static btRigidBody s_fixed(0, 0,0);
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+:m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_constraintType (type),
+m_rbA(s_fixed),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.))
+{
+	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_constraintType (type),
+m_rbA(rbA),
+m_rbB(s_fixed),
+m_appliedImpulse(btScalar(0.))
+{
+		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+
+}
+
+
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+:m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_constraintType (type),
+m_rbA(rbA),
+m_rbB(rbB),
+m_appliedImpulse(btScalar(0.))
+{
+		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+
+}
+
Index: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 1963)
@@ -0,0 +1,120 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TYPED_CONSTRAINT_H
+#define TYPED_CONSTRAINT_H
+
+class btRigidBody;
+#include "LinearMath/btScalar.h"
+
+enum btTypedConstraintType
+{
+	POINT2POINT_CONSTRAINT_TYPE,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	VEHICLE_CONSTRAINT_TYPE,
+	SLIDER_CONSTRAINT_TYPE
+};
+
+///TypedConstraint is the baseclass for Bullet constraints and vehicles
+class btTypedConstraint
+{
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+
+	btTypedConstraintType m_constraintType;
+
+	btTypedConstraint&	operator=(btTypedConstraint&	other)
+	{
+		btAssert(0);
+		(void) other;
+		return *this;
+	}
+
+protected:
+	btRigidBody&	m_rbA;
+	btRigidBody&	m_rbB;
+	btScalar	m_appliedImpulse;
+
+
+public:
+
+	btTypedConstraint(btTypedConstraintType type);
+	virtual ~btTypedConstraint() {};
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+
+	virtual void	buildJacobian() = 0;
+
+	virtual	void	solveConstraint(btScalar	timeStep) = 0;
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	btRigidBody& getRigidBodyA() 
+	{
+		return m_rbA;
+	}
+	btRigidBody& getRigidBodyB()
+	{
+		return m_rbB;
+	}
+
+	int getUserConstraintType() const
+	{
+		return m_userConstraintType ;
+	}
+
+	void	setUserConstraintType(int userConstraintType)
+	{
+		m_userConstraintType = userConstraintType;
+	};
+
+	void	setUserConstraintId(int uid)
+	{
+		m_userConstraintId = uid;
+	}
+	
+	int getUserConstraintId() const
+	{
+		return m_userConstraintId;
+	}
+
+	int getUid() const
+	{
+		return m_userConstraintId;   
+	} 
+
+	btScalar	getAppliedImpulse() const
+	{
+		return m_appliedImpulse;
+	}
+
+	btTypedConstraintType getConstraintType () const
+	{
+		return m_constraintType;
+	}
+
+};
+
+#endif //TYPED_CONSTRAINT_H
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp	(revision 1963)
@@ -0,0 +1,400 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
+	Work in progress, functionality will be added on demand.
+
+	If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
+*/
+
+#include "Bullet-C-Api.h"
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btAlignedAllocator.h"
+
+
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"	
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "LinearMath/btStackAlloc.h"
+
+/*
+	Create and Delete a Physics SDK	
+*/
+
+struct	btPhysicsSdk
+{
+
+//	btDispatcher*				m_dispatcher;
+//	btOverlappingPairCache*		m_pairCache;
+//	btConstraintSolver*			m_constraintSolver
+
+	btVector3	m_worldAabbMin;
+	btVector3	m_worldAabbMax;
+
+
+	//todo: version, hardware/optimization settings etc?
+	btPhysicsSdk()
+		:m_worldAabbMin(-1000,-1000,-1000),
+		m_worldAabbMax(1000,1000,1000)
+	{
+
+	}
+
+	
+};
+
+plPhysicsSdkHandle	plNewBulletSdk()
+{
+	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
+	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
+}
+
+void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
+{
+	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
+	btAlignedFree(phys);	
+}
+
+
+/* Dynamics World */
+plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
+{
+	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
+	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
+	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
+	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
+	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
+	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
+	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
+	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
+
+	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
+	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
+}
+void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
+{
+	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	btAlignedFree(dynamicsWorld);
+}
+
+void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	dynamicsWorld->stepSimulation(timeStep);
+}
+
+void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->addRigidBody(body);
+}
+
+void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->removeRigidBody(body);
+}
+
+/* Rigid Body  */
+
+plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
+{
+	btTransform trans;
+	trans.setIdentity();
+	btVector3 localInertia(0,0,0);
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	if (mass)
+	{
+		shape->calculateLocalInertia(mass,localInertia);
+	}
+	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
+	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
+	btRigidBody* body = new (mem)btRigidBody(rbci);
+	body->setWorldTransform(trans);
+	body->setUserPointer(user_data);
+	return (plRigidBodyHandle) body;
+}
+
+void plDeleteRigidBody(plRigidBodyHandle cbody)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
+	assert(body);
+	btAlignedFree( body);
+}
+
+
+/* Collision Shape definition */
+
+plCollisionShapeHandle plNewSphereShape(plReal radius)
+{
+	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
+	
+}
+	
+plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
+{
+	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
+	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
+}
+
+plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
+{
+	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
+	btVector3 inertiaHalfExtents(radius,height,radius);
+	const int numSpheres = 2;
+	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
+	btScalar radi[numSpheres] = {radius,radius};
+	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+}
+plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
+	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
+}
+
+plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
+	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
+}
+
+/* Convex Meshes */
+plCollisionShapeHandle plNewConvexHullShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
+	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
+}
+
+
+/* Concave static triangle meshes */
+plMeshInterfaceHandle		   plNewMeshInterface()
+{
+	return 0;
+}
+
+plCollisionShapeHandle plNewCompoundShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
+	return (plCollisionShapeHandle) new (mem)btCompoundShape();
+}
+
+void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
+	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
+	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
+	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
+	btTransform	localTrans;
+	localTrans.setIdentity();
+	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
+	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
+	compoundShape->addChildShape(localTrans,childShape);
+}
+
+void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
+{
+	btQuaternion orn;
+	orn.setEuler(yaw,pitch,roll);
+	orient[0] = orn.getX();
+	orient[1] = orn.getY();
+	orient[2] = orn.getZ();
+	orient[3] = orn.getW();
+
+}
+
+
+//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+
+
+void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
+	(void)colShape;
+	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
+	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
+	convexHullShape->addPoint(btPoint3(x,y,z));
+
+}
+
+void plDeleteShape(plCollisionShapeHandle cshape)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btAlignedFree(shape);
+}
+void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
+	shape->setLocalScaling(scaling);	
+}
+
+
+
+void plSetPosition(plRigidBodyHandle object, const plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btVector3 pos(position[0],position[1],position[2]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setOrigin(pos);
+	body->setWorldTransform(worldTrans);
+}
+
+void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setRotation(orn);
+	body->setWorldTransform(worldTrans);
+}
+
+void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	body->getWorldTransform().getOpenGLMatrix(matrix);
+
+}
+
+void	plGetPosition(plRigidBodyHandle object,plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btVector3& pos = body->getWorldTransform().getOrigin();
+	position[0] = pos.getX();
+	position[1] = pos.getY();
+	position[2] = pos.getZ();
+}
+
+void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btQuaternion& orn = body->getWorldTransform().getRotation();
+	orientation[0] = orn.getX();
+	orientation[1] = orn.getY();
+	orientation[2] = orn.getZ();
+	orientation[3] = orn.getW();
+}
+
+
+
+//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
+
+//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
+
+double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
+{
+	btVector3 vp(p1[0], p1[1], p1[2]);
+	btTriangleShape trishapeA(vp, 
+				  btVector3(p2[0], p2[1], p2[2]), 
+				  btVector3(p3[0], p3[1], p3[2]));
+	trishapeA.setMargin(0.000001f);
+	btVector3 vq(q1[0], q1[1], q1[2]);
+	btTriangleShape trishapeB(vq, 
+				  btVector3(q2[0], q2[1], q2[2]), 
+				  btVector3(q3[0], q3[1], q3[2]));
+	trishapeB.setMargin(0.000001f);
+	
+	// btVoronoiSimplexSolver sGjkSimplexSolver;
+	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
+	
+	static btSimplexSolverInterface sGjkSimplexSolver;
+	sGjkSimplexSolver.reset();
+	
+	static btGjkEpaPenetrationDepthSolver Solver0;
+	static btMinkowskiPenetrationDepthSolver Solver1;
+		
+	btConvexPenetrationDepthSolver* Solver = NULL;
+	
+	Solver = &Solver1;	
+		
+	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
+	
+	convexConvex.m_catchDegeneracies = 1;
+	
+	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
+	
+	btPointCollector gjkOutput;
+	btGjkPairDetector::ClosestPointInput input;
+	
+	btStackAlloc gStackAlloc(1024*1024*2);
+ 
+	input.m_stackAlloc = &gStackAlloc;
+	
+	btTransform tr;
+	tr.setIdentity();
+	
+	input.m_transformA = tr;
+	input.m_transformB = tr;
+	
+	convexConvex.getClosestPoints(input, gjkOutput, 0);
+	
+	
+	if (gjkOutput.m_hasResult)
+	{
+		
+		pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
+		pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
+		pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
+
+		pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
+		pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
+		pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
+		
+		normal[0] = gjkOutput.m_normalOnBInWorld[0];
+		normal[1] = gjkOutput.m_normalOnBInWorld[1];
+		normal[2] = gjkOutput.m_normalOnBInWorld[2];
+
+		return gjkOutput.m_distance;
+	}
+	return -1.0f;	
+}
+
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp	(revision 1963)
@@ -0,0 +1,193 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btContinuousDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+
+#include <stdio.h>
+
+btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+}
+
+btContinuousDynamicsWorld::~btContinuousDynamicsWorld()
+{
+}
+
+	
+void	btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+	
+	startProfiling(timeStep);
+	
+
+	///update aabbs information
+	updateAabbs();
+	//static int frame=0;
+//	printf("frame %d\n",frame++);
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	calculateSimulationIslands();
+
+	
+	getSolverInfo().m_timeStep = timeStep;
+	
+
+
+	///solve contact and other joint constraints
+	solveConstraints(getSolverInfo());
+	
+	///CallbackTriggers();
+	calculateTimeOfImpacts(timeStep);
+
+	btScalar toi = dispatchInfo.m_timeOfImpact;
+//	if (toi < 1.f)
+//		printf("toi = %f\n",toi);
+	if (toi < 0.f)
+		printf("toi = %f\n",toi);
+
+
+	///integrate transforms
+	integrateTransforms(timeStep * toi);
+
+	///update vehicle simulation
+	updateVehicles(timeStep);
+
+
+	updateActivationState( timeStep );
+	
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}
+}
+
+void	btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
+{
+		///these should be 'temporal' aabbs!
+		updateTemporalAabbs(timeStep);
+		
+		///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
+		///so we handle the case moving versus static properly, and we cheat for moving versus moving
+		btScalar toi = 1.f;
+		
+	
+		btDispatcherInfo& dispatchInfo = getDispatchInfo();
+		dispatchInfo.m_timeStep = timeStep;
+		dispatchInfo.m_timeOfImpact = 1.f;
+		dispatchInfo.m_stepCount = 0;
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
+
+		///calculate time of impact for overlapping pairs
+
+
+		btDispatcher* dispatcher = getDispatcher();
+		if (dispatcher)
+			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+
+		toi = dispatchInfo.m_timeOfImpact;
+
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
+
+}
+
+void	btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
+{
+
+	btVector3 temporalAabbMin,temporalAabbMax;
+
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax);
+			const btVector3& linvel = body->getLinearVelocity();
+
+			//make the AABB temporal
+			btScalar temporalAabbMaxx = temporalAabbMax.getX();
+			btScalar temporalAabbMaxy = temporalAabbMax.getY();
+			btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+			btScalar temporalAabbMinx = temporalAabbMin.getX();
+			btScalar temporalAabbMiny = temporalAabbMin.getY();
+			btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+			// add linear motion
+			btVector3 linMotion = linvel*timeStep;
+		
+			if (linMotion.x() > 0.f)
+				temporalAabbMaxx += linMotion.x(); 
+			else
+				temporalAabbMinx += linMotion.x();
+			if (linMotion.y() > 0.f)
+				temporalAabbMaxy += linMotion.y(); 
+			else
+				temporalAabbMiny += linMotion.y();
+			if (linMotion.z() > 0.f)
+				temporalAabbMaxz += linMotion.z(); 
+			else
+				temporalAabbMinz += linMotion.z();
+
+			//add conservative angular motion
+			btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep;
+			btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+			temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+			temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+			temporalAabbMin -= angularMotion3d;
+			temporalAabbMax += angularMotion3d;
+
+			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
+		}
+	}
+
+	//update aabb (of all moved objects)
+
+	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+	
+
+
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h	(revision 1963)
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H
+#define BT_CONTINUOUS_DYNAMICS_WORLD_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld.
+///This copes with fast moving objects that otherwise would tunnel/miss collisions.
+///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead.
+class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+	void	updateTemporalAabbs(btScalar timeStep);
+
+	public:
+
+		btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+		virtual ~btContinuousDynamicsWorld();
+		
+		///time stepping with calculation of time of impact for selected fast moving objects
+		virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+		virtual void	calculateTimeOfImpacts(btScalar timeStep);
+
+		virtual btDynamicsWorldType	getWorldType() const
+		{
+			return BT_CONTINUOUS_DYNAMICS_WORLD;
+		}
+
+};
+
+#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp	(revision 1963)
@@ -0,0 +1,1173 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btDiscreteDynamicsWorld.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "LinearMath/btIDebugDraw.h"
+
+
+
+//vehicle
+#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
+#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
+#include "BulletDynamics/Vehicle/btWheelInfo.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
+
+
+
+
+
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
+m_gravity(0,-10,0),
+m_localTime(btScalar(1.)/btScalar(60.)),
+m_profileTimings(0)
+{
+	if (!m_constraintSolver)
+	{
+		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+		m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
+		m_ownsConstraintSolver = true;
+	} else
+	{
+		m_ownsConstraintSolver = false;
+	}
+
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+		m_islandManager = new (mem) btSimulationIslandManager();
+	}
+
+	m_ownsIslandManager = true;
+}
+
+
+btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
+{
+	//only delete it when we created it
+	if (m_ownsIslandManager)
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree( m_islandManager);
+	}
+	if (m_ownsConstraintSolver)
+	{
+
+		m_constraintSolver->~btConstraintSolver();
+		btAlignedFree(m_constraintSolver);
+	}
+}
+
+void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+
+	for (int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+				btTransform predictedTrans;
+				if (body->getActivationState() != ISLAND_SLEEPING)
+				{
+					if (body->isKinematicObject())
+					{
+						//to calculate velocities next frame
+						body->saveKinematicState(timeStep);
+					}
+				}
+		}
+	}
+}
+
+void	btDiscreteDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("debugDrawWorld");
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		btVector3 color(0,0,0);
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
+			{
+				btManifoldPoint& cp = contactManifold->getContactPoint(j);
+				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+			}
+		}
+	}
+
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
+	{
+		int i;
+
+		//todo: iterate over awake simulation islands!
+		for (  i=0;i<m_collisionObjects.size();i++)
+		{
+			btCollisionObject* colObj = m_collisionObjects[i];
+			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+			{
+				btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
+				switch(colObj->getActivationState())
+				{
+				case  ACTIVE_TAG:
+					color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
+				case ISLAND_SLEEPING:
+					color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
+				case WANTS_DEACTIVATION:
+					color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
+				case DISABLE_DEACTIVATION:
+					color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
+				case DISABLE_SIMULATION:
+					color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
+				default:
+					{
+						color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
+					}
+				};
+
+				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+			}
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				btPoint3 minAabb,maxAabb;
+				btVector3 colorvec(1,0,0);
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+			}
+
+		}
+	
+		for (  i=0;i<this->m_vehicles.size();i++)
+		{
+			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+			{
+				btVector3 wheelColor(0,255,255);
+				if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
+				{
+					wheelColor.setValue(0,0,255);
+				} else
+				{
+					wheelColor.setValue(255,0,255);
+				}
+		
+				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
+
+				btVector3 axle = btVector3(	
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
+					m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
+
+
+				//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
+				//debug wheels (cylinders)
+				m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
+				m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
+
+			}
+		}
+	}
+}
+
+void	btDiscreteDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
+///apply gravity, call this once per timestep
+void	btDiscreteDynamicsWorld::applyGravity()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->isActive())
+		{
+			body->applyGravity();
+		}
+	}
+}
+
+
+
+void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+{
+	BT_PROFILE("synchronizeMotionStates");
+	{
+		//todo: iterate over awake simulation islands!
+		for ( int i=0;i<m_collisionObjects.size();i++)
+		{
+			btCollisionObject* colObj = m_collisionObjects[i];
+			
+			btRigidBody* body = btRigidBody::upcast(colObj);
+			if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+			{
+				//we need to call the update at least once, even for sleeping objects
+				//otherwise the 'graphics' transform never updates properly
+				//so todo: add 'dirty' flag
+				//if (body->getActivationState() != ISLAND_SLEEPING)
+				{
+					btTransform interpolatedTransform;
+					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
+					body->getMotionState()->setWorldTransform(interpolatedTransform);
+				}
+			}
+		}
+	}
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+	{
+		for ( int i=0;i<this->m_vehicles.size();i++)
+		{
+			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+			{
+				//synchronize the wheels with the (interpolated) chassis worldtransform
+				m_vehicles[i]->updateWheelTransform(v,true);
+			}
+		}
+	}
+
+}
+
+
+int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+{
+	startProfiling(timeStep);
+
+	BT_PROFILE("stepSimulation");
+
+	int numSimulationSubSteps = 0;
+
+	if (maxSubSteps)
+	{
+		//fixed timestep with interpolation
+		m_localTime += timeStep;
+		if (m_localTime >= fixedTimeStep)
+		{
+			numSimulationSubSteps = int( m_localTime / fixedTimeStep);
+			m_localTime -= numSimulationSubSteps * fixedTimeStep;
+		}
+	} else
+	{
+		//variable timestep
+		fixedTimeStep = timeStep;
+		m_localTime = timeStep;
+		if (btFuzzyZero(timeStep))
+		{
+			numSimulationSubSteps = 0;
+			maxSubSteps = 0;
+		} else
+		{
+			numSimulationSubSteps = 1;
+			maxSubSteps = 1;
+		}
+	}
+
+	//process some debugging flags
+	if (getDebugDrawer())
+	{
+		gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+	}
+	if (numSimulationSubSteps)
+	{
+
+		saveKinematicState(fixedTimeStep);
+
+		applyGravity();
+
+		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+
+		for (int i=0;i<clampedSimulationSteps;i++)
+		{
+			internalSingleStepSimulation(fixedTimeStep);
+			synchronizeMotionStates();
+		}
+
+	} 
+
+	synchronizeMotionStates();
+
+	clearForces();
+
+#ifndef BT_NO_PROFILE
+	CProfileManager::Increment_Frame_Counter();
+#endif //BT_NO_PROFILE
+	
+	return numSimulationSubSteps;
+}
+
+void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+	
+	BT_PROFILE("internalSingleStepSimulation");
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	calculateSimulationIslands();
+
+	
+	getSolverInfo().m_timeStep = timeStep;
+	
+
+
+	///solve contact and other joint constraints
+	solveConstraints(getSolverInfo());
+	
+	///CallbackTriggers();
+
+	///integrate transforms
+	integrateTransforms(timeStep);
+
+	///update vehicle simulation
+	updateVehicles(timeStep);
+
+
+	updateActivationState( timeStep );
+
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}	
+}
+
+void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	m_gravity = gravity;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setGravity(gravity);
+		}
+	}
+}
+
+btVector3 btDiscreteDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
+
+void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+	removeCollisionObject(body);
+}
+
+void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+	if (!body->isStaticOrKinematicObject())
+	{
+		body->setGravity(m_gravity);
+	}
+
+	if (body->getCollisionShape())
+	{
+		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
+		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+		addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+	}
+}
+
+void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+{
+	if (!body->isStaticOrKinematicObject())
+	{
+		body->setGravity(m_gravity);
+	}
+
+	if (body->getCollisionShape())
+	{
+		addCollisionObject(body,group,mask);
+	}
+}
+
+
+void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
+{
+	BT_PROFILE("updateVehicles");
+	
+	for ( int i=0;i<m_vehicles.size();i++)
+	{
+		btRaycastVehicle* vehicle = m_vehicles[i];
+		vehicle->updateVehicle( timeStep);
+	}
+}
+
+void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
+{
+	BT_PROFILE("updateActivationState");
+
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->updateDeactivation(timeStep);
+
+			if (body->wantsSleeping())
+			{
+				if (body->isStaticOrKinematicObject())
+				{
+					body->setActivationState(ISLAND_SLEEPING);
+				} else
+				{
+					if (body->getActivationState() == ACTIVE_TAG)
+						body->setActivationState( WANTS_DEACTIVATION );
+					if (body->getActivationState() == ISLAND_SLEEPING) 
+					{
+						body->setAngularVelocity(btVector3(0,0,0));
+						body->setLinearVelocity(btVector3(0,0,0));
+					}
+
+				}
+			} else
+			{
+				if (body->getActivationState() != DISABLE_DEACTIVATION)
+					body->setActivationState( ACTIVE_TAG );
+			}
+		}
+	}
+}
+
+void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
+{
+	m_constraints.push_back(constraint);
+	if (disableCollisionsBetweenLinkedBodies)
+	{
+		constraint->getRigidBodyA().addConstraintRef(constraint);
+		constraint->getRigidBodyB().addConstraintRef(constraint);
+	}
+}
+
+void	btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
+{
+	m_constraints.remove(constraint);
+	constraint->getRigidBodyA().removeConstraintRef(constraint);
+	constraint->getRigidBodyB().removeConstraintRef(constraint);
+}
+
+void	btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
+{
+	m_vehicles.push_back(vehicle);
+}
+
+void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
+{
+	m_vehicles.remove(vehicle);
+}
+
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+{
+	int islandId;
+	
+	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
+	return islandId;
+
+}
+
+
+class btSortConstraintOnIslandPredicate
+{
+	public:
+
+		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
+		{
+			int rIslandId0,lIslandId0;
+			rIslandId0 = btGetConstraintIslandId(rhs);
+			lIslandId0 = btGetConstraintIslandId(lhs);
+			return lIslandId0 < rIslandId0;
+		}
+};
+
+
+
+
+void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+{
+	BT_PROFILE("solveConstraints");
+	
+	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+	{
+
+		btContactSolverInfo&	m_solverInfo;
+		btConstraintSolver*		m_solver;
+		btTypedConstraint**		m_sortedConstraints;
+		int						m_numConstraints;
+		btIDebugDraw*			m_debugDrawer;
+		btStackAlloc*			m_stackAlloc;
+		btDispatcher*			m_dispatcher;
+
+		InplaceSolverIslandCallback(
+			btContactSolverInfo& solverInfo,
+			btConstraintSolver*	solver,
+			btTypedConstraint** sortedConstraints,
+			int	numConstraints,
+			btIDebugDraw*	debugDrawer,
+			btStackAlloc*			stackAlloc,
+			btDispatcher* dispatcher)
+			:m_solverInfo(solverInfo),
+			m_solver(solver),
+			m_sortedConstraints(sortedConstraints),
+			m_numConstraints(numConstraints),
+			m_debugDrawer(debugDrawer),
+			m_stackAlloc(stackAlloc),
+			m_dispatcher(dispatcher)
+		{
+
+		}
+
+		InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
+		{
+			btAssert(0);
+			(void)other;
+			return *this;
+		}
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
+		{
+			if (islandId<0)
+			{
+				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			} else
+			{
+					//also add all non-contact constraints/joints for this island
+				btTypedConstraint** startConstraint = 0;
+				int numCurConstraints = 0;
+				int i;
+				
+				//find the first constraint for this island
+				for (i=0;i<m_numConstraints;i++)
+				{
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						startConstraint = &m_sortedConstraints[i];
+						break;
+					}
+				}
+				//count the number of constraints in this island
+				for (;i<m_numConstraints;i++)
+				{
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						numCurConstraints++;
+					}
+				}
+
+				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+				if (numManifolds + numCurConstraints)
+				{
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
+		
+			}
+		}
+
+	};
+
+	//sorted version of all btTypedConstraint, based on islandId
+	btAlignedObjectArray<btTypedConstraint*>	sortedConstraints;
+	sortedConstraints.resize( m_constraints.size());
+	int i; 
+	for (i=0;i<getNumConstraints();i++)
+	{
+		sortedConstraints[i] = m_constraints[i];
+	}
+
+//	assert(0);
+		
+	
+
+	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
+	
+	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
+	
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
+	
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+	
+	/// solve all the constraints for this island
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
+}
+
+
+
+
+void	btDiscreteDynamicsWorld::calculateSimulationIslands()
+{
+	BT_PROFILE("calculateSimulationIslands");
+
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+
+	{
+		int i;
+		int numConstraints = int(m_constraints.size());
+		for (i=0;i< numConstraints ; i++ )
+		{
+			btTypedConstraint* constraint = m_constraints[i];
+
+			const btRigidBody* colObj0 = &constraint->getRigidBodyA();
+			const btRigidBody* colObj1 = &constraint->getRigidBodyB();
+
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				if (colObj0->isActive() || colObj1->isActive())
+				{
+
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
+						(colObj1)->getIslandTag());
+				}
+			}
+		}
+	}
+
+	//Store the island id in each body
+	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
+
+	
+}
+
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
+{
+	btCollisionObject* m_me;
+	btScalar m_allowedPenetration;
+	btOverlappingPairCache* m_pairCache;
+
+
+public:
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
+	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+		m_allowedPenetration(0.0f),
+		m_me(me),
+		m_pairCache(pairCache)
+	{
+	}
+
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	{
+		if (convexResult.m_hitCollisionObject == m_me)
+			return 1.0;
+
+		btVector3 linVelA,linVelB;
+		linVelA = m_convexToWorld-m_convexFromWorld;
+		linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+
+		btVector3 relativeVelocity = (linVelA-linVelB);
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+			return 1.f;
+
+		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+	}
+
+	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+	{
+		//don't collide with itself
+		if (proxy0->m_clientObject == m_me)
+			return false;
+
+		///don't do CCD when the collision filters are not matching
+		if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
+			return false;
+
+		///don't do CCD when there are already contact points (touching contact/penetration)
+		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+		btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+		if (collisionPair)
+		{
+			if (collisionPair->m_algorithm)
+			{
+				manifoldArray.resize(0);
+				collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+				for (int j=0;j<manifoldArray.size();j++)
+				{
+					btPersistentManifold* manifold = manifoldArray[j];
+					if (manifold->getNumContacts()>0)
+						return false;
+				}
+			}
+		}
+		return true;
+	}
+
+
+};
+
+///internal debugging variable. this value shouldn't be too high
+int gNumClampedCcdMotions=0;
+
+//#include "stdio.h"
+void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setHitFraction(1.f);
+
+			if (body->isActive() && (!body->isStaticOrKinematicObject()))
+			{
+				body->predictIntegratedTransform(timeStep, predictedTrans);
+				btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+
+				if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+				{
+					BT_PROFILE("CCD motion clamping");
+					if (body->getCollisionShape()->isConvex())
+					{
+						gNumClampedCcdMotions++;
+						
+						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+						btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+						btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+						convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+						if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+						{
+							body->setHitFraction(sweepResults.m_closestHitFraction);
+							body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+							body->setHitFraction(0.f);
+//							printf("clamped integration to hit fraction = %f\n",fraction);
+						}
+					}
+				}
+				
+				body->proceedToTransform( predictedTrans);
+			}
+		}
+	}
+}
+
+
+
+void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	BT_PROFILE("predictUnconstraintMotion");
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (!body->isStaticOrKinematicObject())
+			{
+				if (body->isActive())
+				{
+					body->integrateVelocities( timeStep);
+					//damping
+					body->applyDamping(timeStep);
+
+					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+				}
+			}
+		}
+	}
+}
+
+
+void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
+{
+	(void)timeStep;
+
+#ifndef BT_NO_PROFILE
+	CProfileManager::Reset();
+#endif //BT_NO_PROFILE
+
+}
+
+
+
+
+	
+
+class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
+	btIDebugDraw*	m_debugDrawer;
+	btVector3	m_color;
+	btTransform	m_worldTrans;
+
+public:
+
+	DebugDrawcallback(btIDebugDraw*	debugDrawer,const btTransform& worldTrans,const btVector3& color) :
+                m_debugDrawer(debugDrawer),
+		m_color(color),
+		m_worldTrans(worldTrans)
+	{
+	}
+
+	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	{
+		processTriangle(triangle,partId,triangleIndex);
+	}
+
+	virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+	{
+		(void)partId;
+		(void)triangleIndex;
+
+		btVector3 wv0,wv1,wv2;
+		wv0 = m_worldTrans*triangle[0];
+		wv1 = m_worldTrans*triangle[1];
+		wv2 = m_worldTrans*triangle[2];
+		m_debugDrawer->drawLine(wv0,wv1,m_color);
+		m_debugDrawer->drawLine(wv1,wv2,m_color);
+		m_debugDrawer->drawLine(wv2,wv0,m_color);
+	}
+};
+
+void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
+	btVector3 start = transform.getOrigin();
+
+	const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
+	const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
+	const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
+
+	// XY 
+	getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
+	getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
+
+	// XZ
+	getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
+	getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
+
+	// YZ
+	getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
+	getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
+	getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
+	getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
+
+void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+	// Draw a small simplex at the center of the object
+	{
+		btVector3 start = worldTransform.getOrigin();
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
+		getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+	}
+
+	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+	{
+		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+		{
+			btTransform childTrans = compoundShape->getChildTransform(i);
+			const btCollisionShape* colShape = compoundShape->getChildShape(i);
+			debugDrawObject(worldTransform*childTrans,colShape,color);
+		}
+
+	} else
+	{
+		switch (shape->getShapeType())
+		{
+
+		case SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+				btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+				
+				debugDrawSphere(radius, worldTransform, color);
+				break;
+			}
+		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+			{
+				const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+				for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
+					debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
+				}
+
+				break;
+			}
+		case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+				btScalar radius = capsuleShape->getRadius();
+				btScalar halfHeight = capsuleShape->getHalfHeight();
+				
+				int upAxis = capsuleShape->getUpAxis();
+
+				
+				btVector3 capStart(0.f,0.f,0.f);
+				capStart[upAxis] = -halfHeight;
+
+				btVector3 capEnd(0.f,0.f,0.f);
+				capEnd[upAxis] = halfHeight;
+
+				// Draw the ends
+				{
+					
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * capStart;
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				{
+					btTransform childTransform = worldTransform;
+					childTransform.getOrigin() = worldTransform * capEnd;
+					debugDrawSphere(radius, childTransform, color);
+				}
+
+				// Draw some additional lines
+				btVector3 start = worldTransform.getOrigin();
+
+				
+				capStart[(upAxis+1)%3] = radius;
+				capEnd[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+1)%3] = -radius;
+				capEnd[(upAxis+1)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				capStart[(upAxis+1)%3] = 0.f;
+				capEnd[(upAxis+1)%3] = 0.f;
+
+				capStart[(upAxis+2)%3] = radius;
+				capEnd[(upAxis+2)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+2)%3] = -radius;
+				capEnd[(upAxis+2)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				
+				break;
+			}
+		case CONE_SHAPE_PROXYTYPE:
+			{
+				const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+				btVector3 start = worldTransform.getOrigin();
+
+				int upAxis= coneShape->getConeUpIndex();
+				
+
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = height * btScalar(0.5);
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				btVector3	offset2Radius(0,0,0);
+				offset2Radius[(upAxis+2)%3] = radius;
+
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+
+
+				break;
+
+			}
+		case CYLINDER_SHAPE_PROXYTYPE:
+			{
+				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+				int upAxis = cylinder->getUpAxis();
+				btScalar radius = cylinder->getRadius();
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+				btVector3 start = worldTransform.getOrigin();
+				btVector3	offsetHeight(0,0,0);
+				offsetHeight[upAxis] = halfHeight;
+				btVector3	offsetRadius(0,0,0);
+				offsetRadius[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+				break;
+			}
+
+			case STATIC_PLANE_PROXYTYPE:
+				{
+					const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+					btScalar planeConst = staticPlaneShape->getPlaneConstant();
+					const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+					btVector3 planeOrigin = planeNormal * planeConst;
+					btVector3 vec0,vec1;
+					btPlaneSpace1(planeNormal,vec0,vec1);
+					btScalar vecLen = 100.f;
+					btVector3 pt0 = planeOrigin + vec0*vecLen;
+					btVector3 pt1 = planeOrigin - vec0*vecLen;
+					btVector3 pt2 = planeOrigin + vec1*vecLen;
+					btVector3 pt3 = planeOrigin - vec1*vecLen;
+					getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+					getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+					break;
+
+				}
+		default:
+			{
+
+				if (shape->isConcave())
+				{
+					btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+					
+					//todo pass camera, for some culling
+					btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+
+					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+					concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+				}
+
+				if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+				{
+					btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+					//todo: pass camera for some culling			
+					btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+					//DebugDrawcallback drawCallback;
+					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+				}
+
+
+				/// for polyhedral shapes
+				if (shape->isPolyhedral())
+				{
+					btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+
+					int i;
+					for (i=0;i<polyshape->getNumEdges();i++)
+					{
+						btPoint3 a,b;
+						polyshape->getEdge(i,a,b);
+						btVector3 wa = worldTransform * a;
+						btVector3 wb = worldTransform * b;
+						getDebugDrawer()->drawLine(wa,wb,color);
+
+					}
+
+					
+				}
+			}
+		}
+	}
+}
+
+
+void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+	if (m_ownsConstraintSolver)
+	{
+		btAlignedFree( m_constraintSolver);
+	}
+	m_ownsConstraintSolver = false;
+	m_constraintSolver = solver;
+}
+
+btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
+
+
+int		btDiscreteDynamicsWorld::getNumConstraints() const
+{
+	return int(m_constraints.size());
+}
+btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
+{
+	return m_constraints[index];
+}
+const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
+{
+	return m_constraints[index];
+}
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h	(revision 1963)
@@ -0,0 +1,159 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DISCRETE_DYNAMICS_WORLD_H
+#define BT_DISCRETE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+class btSimulationIslandManager;
+class btTypedConstraint;
+
+
+class btRaycastVehicle;
+class btIDebugDraw;
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+///btDiscreteDynamicsWorld provides discrete rigid body simulation
+///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+class btDiscreteDynamicsWorld : public btDynamicsWorld
+{
+protected:
+
+	btConstraintSolver*	m_constraintSolver;
+
+	btSimulationIslandManager*	m_islandManager;
+
+	btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
+
+	btVector3	m_gravity;
+
+	//for variable timesteps
+	btScalar	m_localTime;
+	//for variable timesteps
+
+	bool	m_ownsIslandManager;
+	bool	m_ownsConstraintSolver;
+
+	
+	btAlignedObjectArray<btRaycastVehicle*>	m_vehicles;
+
+	int	m_profileTimings;
+
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+	
+	virtual void	integrateTransforms(btScalar timeStep);
+		
+	void	calculateSimulationIslands();
+
+	void	solveConstraints(btContactSolverInfo& solverInfo);
+	
+	void	updateActivationState(btScalar timeStep);
+
+	void	updateVehicles(btScalar timeStep);
+
+	void	startProfiling(btScalar timeStep);
+
+	virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+	void	synchronizeMotionStates();
+
+	void	saveKinematicState(btScalar timeStep);
+
+	void	debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+
+public:
+
+
+	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
+	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btDiscreteDynamicsWorld();
+
+	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
+	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+
+	void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
+
+	void	removeConstraint(btTypedConstraint* constraint);
+
+	void	addVehicle(btRaycastVehicle* vehicle);
+
+	void	removeVehicle(btRaycastVehicle* vehicle);
+
+	btSimulationIslandManager*	getSimulationIslandManager()
+	{
+		return m_islandManager;
+	}
+
+	const btSimulationIslandManager*	getSimulationIslandManager() const 
+	{
+		return m_islandManager;
+	}
+
+	btCollisionWorld*	getCollisionWorld()
+	{
+		return this;
+	}
+
+
+	virtual void	setGravity(const btVector3& gravity);
+	virtual btVector3 getGravity () const;
+
+	virtual void	addRigidBody(btRigidBody* body);
+
+	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+
+	virtual void	removeRigidBody(btRigidBody* body);
+
+	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+	virtual void	debugDrawWorld();
+
+	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
+	
+	virtual	int		getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index)	;
+
+	virtual const btTypedConstraint* getConstraint(int index) const;
+
+	
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_DISCRETE_DYNAMICS_WORLD;
+	}
+	
+	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
+	virtual void	clearForces();
+
+	///apply gravity, call this once per timestep
+	virtual void	applyGravity();
+
+	virtual void	setNumTasks(int numTasks)
+	{
+	}
+
+};
+
+#endif //BT_DISCRETE_DYNAMICS_WORLD_H
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h	(revision 1963)
@@ -0,0 +1,128 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DYNAMICS_WORLD_H
+#define BT_DYNAMICS_WORLD_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+class btTypedConstraint;
+class btRaycastVehicle;
+class btConstraintSolver;
+class btDynamicsWorld;
+
+/// Type for the callback for each tick
+typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
+
+enum btDynamicsWorldType
+{
+	BT_SIMPLE_DYNAMICS_WORLD=1,
+	BT_DISCRETE_DYNAMICS_WORLD=2,
+	BT_CONTINUOUS_DYNAMICS_WORLD=3
+};
+
+///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
+class btDynamicsWorld : public btCollisionWorld
+{
+
+protected:
+		btInternalTickCallback m_internalTickCallback;
+		void*	m_worldUserInfo;
+
+		btContactSolverInfo	m_solverInfo;
+
+public:
+		
+
+		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
+		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
+		{
+		}
+
+		virtual ~btDynamicsWorld()
+		{
+		}
+		
+		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+			
+		virtual void	debugDrawWorld() = 0;
+				
+		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
+		{ 
+			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
+		}
+
+		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
+
+		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
+
+		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
+
+		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
+		//existing rigidbodies in the world get gravity assigned too, during this method
+		virtual void	setGravity(const btVector3& gravity) = 0;
+		virtual btVector3 getGravity () const = 0;
+
+		virtual void	addRigidBody(btRigidBody* body) = 0;
+
+		virtual void	removeRigidBody(btRigidBody* body) = 0;
+
+		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+
+		virtual btConstraintSolver* getConstraintSolver() = 0;
+		
+		virtual	int		getNumConstraints() const {	return 0;		}
+		
+		virtual btTypedConstraint* getConstraint(int index)		{	(void)index;		return 0;		}
+		
+		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
+
+		virtual btDynamicsWorldType	getWorldType() const=0;
+
+		virtual void	clearForces() = 0;
+
+		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0) 
+		{ 
+			m_internalTickCallback = cb; 
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void	setWorldUserInfo(void* worldUserInfo)
+		{
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void*	getWorldUserInfo() const
+		{
+			return m_worldUserInfo;
+		}
+
+		btContactSolverInfo& getSolverInfo()
+		{
+			return m_solverInfo;
+		}
+
+
+};
+
+#endif //BT_DYNAMICS_WORLD_H
+
+
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp	(revision 1963)
@@ -0,0 +1,314 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btRigidBody.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMotionState.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+//'temporarily' global variables
+btScalar	gDeactivationTime = btScalar(2.);
+bool	gDisableDeactivation = false;
+static int uniqueId = 0;
+
+
+btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+	setupRigidBody(constructionInfo);
+}
+
+btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+{
+	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	setupRigidBody(cinfo);
+}
+
+void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+
+	m_internalType=CO_RIGID_BODY;
+
+	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	m_angularFactor = btScalar(1.);
+	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
+	m_linearDamping = btScalar(0.);
+	m_angularDamping = btScalar(0.5);
+	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
+	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
+	m_optionalMotionState = constructionInfo.m_motionState;
+	m_contactSolverType = 0;
+	m_frictionSolverType = 0;
+	m_additionalDamping = constructionInfo.m_additionalDamping;
+	m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
+	m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
+	m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
+	m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
+
+	if (m_optionalMotionState)
+	{
+		m_optionalMotionState->getWorldTransform(m_worldTransform);
+	} else
+	{
+		m_worldTransform = constructionInfo.m_startWorldTransform;
+	}
+
+	m_interpolationWorldTransform = m_worldTransform;
+	m_interpolationLinearVelocity.setValue(0,0,0);
+	m_interpolationAngularVelocity.setValue(0,0,0);
+	
+	//moved to btCollisionObject
+	m_friction = constructionInfo.m_friction;
+	m_restitution = constructionInfo.m_restitution;
+
+	setCollisionShape( constructionInfo.m_collisionShape );
+	m_debugBodyId = uniqueId++;
+	
+	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
+    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
+	updateInertiaTensor();
+
+}
+
+
+void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
+{
+	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
+}
+
+void			btRigidBody::saveKinematicState(btScalar timeStep)
+{
+	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+	if (timeStep != btScalar(0.))
+	{
+		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+		if (getMotionState())
+			getMotionState()->getWorldTransform(m_worldTransform);
+		btVector3 linVel,angVel;
+		
+		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		m_interpolationLinearVelocity = m_linearVelocity;
+		m_interpolationAngularVelocity = m_angularVelocity;
+		m_interpolationWorldTransform = m_worldTransform;
+		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
+	}
+}
+	
+void	btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+{
+	getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
+}
+
+
+
+
+void btRigidBody::setGravity(const btVector3& acceleration) 
+{
+	if (m_inverseMass != btScalar(0.0))
+	{
+		m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
+	}
+}
+
+
+
+
+
+
+void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
+{
+	m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+}
+
+
+
+
+///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
+void			btRigidBody::applyDamping(btScalar timeStep)
+{
+	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
+	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
+
+//#define USE_OLD_DAMPING_METHOD 1
+#ifdef USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#else
+	m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
+	m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
+#endif
+
+	if (m_additionalDamping)
+	{
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
+			(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
+		{
+			m_angularVelocity *= m_additionalDampingFactor;
+			m_linearVelocity *= m_additionalDampingFactor;
+		}
+	
+
+		btScalar speed = m_linearVelocity.length();
+		if (speed < m_linearDamping)
+		{
+			btScalar dampVel = btScalar(0.005);
+			if (speed > dampVel)
+			{
+				btVector3 dir = m_linearVelocity.normalized();
+				m_linearVelocity -=  dir * dampVel;
+			} else
+			{
+				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+
+		btScalar angSpeed = m_angularVelocity.length();
+		if (angSpeed < m_angularDamping)
+		{
+			btScalar angDampVel = btScalar(0.005);
+			if (angSpeed > angDampVel)
+			{
+				btVector3 dir = m_angularVelocity.normalized();
+				m_angularVelocity -=  dir * angDampVel;
+			} else
+			{
+				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+	}
+}
+
+
+void btRigidBody::applyGravity()
+{
+	if (isStaticOrKinematicObject())
+		return;
+	
+	applyCentralForce(m_gravity);	
+
+}
+
+void btRigidBody::proceedToTransform(const btTransform& newTrans)
+{
+	setCenterOfMassTransform( newTrans );
+}
+	
+
+void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
+{
+	if (mass == btScalar(0.))
+	{
+		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
+		m_inverseMass = btScalar(0.);
+	} else
+	{
+		m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+		m_inverseMass = btScalar(1.0) / mass;
+	}
+	
+	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
+				   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
+				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+
+}
+
+	
+
+void btRigidBody::updateInertiaTensor() 
+{
+	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
+}
+
+
+void btRigidBody::integrateVelocities(btScalar step) 
+{
+	if (isStaticOrKinematicObject())
+		return;
+
+	m_linearVelocity += m_totalForce * (m_inverseMass * step);
+	m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
+
+#define MAX_ANGVEL SIMD_HALF_PI
+	/// clamp angular velocity. collision calculations will fail on higher angular velocities	
+	btScalar angvel = m_angularVelocity.length();
+	if (angvel*step > MAX_ANGVEL)
+	{
+		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
+	}
+
+}
+
+btQuaternion btRigidBody::getOrientation() const
+{
+		btQuaternion orn;
+		m_worldTransform.getBasis().getRotation(orn);
+		return orn;
+}
+	
+	
+void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
+{
+
+	if (isStaticOrKinematicObject())
+	{
+		m_interpolationWorldTransform = m_worldTransform;
+	} else
+	{
+		m_interpolationWorldTransform = xform;
+	}
+	m_interpolationLinearVelocity = getLinearVelocity();
+	m_interpolationAngularVelocity = getAngularVelocity();
+	m_worldTransform = xform;
+	updateInertiaTensor();
+}
+
+
+bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
+{
+	btRigidBody* otherRb = btRigidBody::upcast(co);
+	if (!otherRb)
+		return true;
+
+	for (int i = 0; i < m_constraintRefs.size(); ++i)
+	{
+		btTypedConstraint* c = m_constraintRefs[i];
+		if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
+			return false;
+	}
+
+	return true;
+}
+
+void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
+	int index = m_constraintRefs.findLinearSearch(c);
+	if (index == m_constraintRefs.size())
+		m_constraintRefs.push_back(c); 
+
+	m_checkCollideWith = true;
+}
+
+void btRigidBody::removeConstraintRef(btTypedConstraint* c)
+{
+	m_constraintRefs.remove(c);
+	m_checkCollideWith = m_constraintRefs.size() > 0;
+}
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.h	(revision 1963)
@@ -0,0 +1,482 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RIGIDBODY_H
+#define RIGIDBODY_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+class btCollisionShape;
+class btMotionState;
+class btTypedConstraint;
+
+
+extern btScalar gDeactivationTime;
+extern bool gDisableDeactivation;
+
+
+///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
+///There are 3 types of rigid bodies: 
+///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
+///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
+///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
+class btRigidBody  : public btCollisionObject
+{
+
+	btMatrix3x3	m_invInertiaTensorWorld;
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
+	btScalar		m_inverseMass;
+	btScalar		m_angularFactor;
+
+	btVector3		m_gravity;	
+	btVector3		m_invInertiaLocal;
+	btVector3		m_totalForce;
+	btVector3		m_totalTorque;
+	
+	btScalar		m_linearDamping;
+	btScalar		m_angularDamping;
+
+	bool			m_additionalDamping;
+	btScalar		m_additionalDampingFactor;
+	btScalar		m_additionalLinearDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingFactor;
+
+
+	btScalar		m_linearSleepingThreshold;
+	btScalar		m_angularSleepingThreshold;
+
+	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	m_optionalMotionState;
+
+	//keep track of typed constraints referencing this rigid body
+	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+
+public:
+
+
+	///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
+	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
+	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
+	///m_startWorldTransform is only used when you don't provide a motion state.
+	struct	btRigidBodyConstructionInfo
+	{
+		btScalar			m_mass;
+
+		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
+		///In this case, m_startWorldTransform is ignored.
+		btMotionState*		m_motionState;
+		btTransform	m_startWorldTransform;
+
+		btCollisionShape*	m_collisionShape;
+		btVector3			m_localInertia;
+		btScalar			m_linearDamping;
+		btScalar			m_angularDamping;
+
+		///best simulation results when friction is non-zero
+		btScalar			m_friction;
+		///best simulation results using zero restitution.
+		btScalar			m_restitution;
+
+		btScalar			m_linearSleepingThreshold;
+		btScalar			m_angularSleepingThreshold;
+
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		bool				m_additionalDamping;
+		btScalar			m_additionalDampingFactor;
+		btScalar			m_additionalLinearDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingFactor;
+
+		
+		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+		m_mass(mass),
+			m_motionState(motionState),
+			m_collisionShape(collisionShape),
+			m_localInertia(localInertia),
+			m_linearDamping(btScalar(0.)),
+			m_angularDamping(btScalar(0.)),
+			m_friction(btScalar(0.5)),
+			m_restitution(btScalar(0.)),
+			m_linearSleepingThreshold(btScalar(0.8)),
+			m_angularSleepingThreshold(btScalar(1.f)),
+			m_additionalDamping(false),
+			m_additionalDampingFactor(btScalar(0.005)),
+			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingFactor(btScalar(0.01))
+		{
+			m_startWorldTransform.setIdentity();
+		}
+	};
+
+	///btRigidBody constructor using construction info
+	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+
+	///btRigidBody constructor for backwards compatibility. 
+	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
+	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
+
+
+	virtual ~btRigidBody()
+        { 
+                //No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
+                btAssert(m_constraintRefs.size()==0); 
+        }
+
+protected:
+
+	///setupRigidBody is only used internally by the constructor
+	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+
+public:
+
+	void			proceedToTransform(const btTransform& newTrans); 
+	
+	///to keep collision detection and dynamics separate we don't store a rigidbody pointer
+	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
+	static const btRigidBody*	upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (const btRigidBody*)colObj;
+		return 0;
+	}
+	static btRigidBody*	upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (btRigidBody*)colObj;
+		return 0;
+	}
+	
+	/// continuous collision detection needs prediction
+	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
+	
+	void			saveKinematicState(btScalar step);
+	
+	void			applyGravity();
+	
+	void			setGravity(const btVector3& acceleration);  
+
+	const btVector3&	getGravity() const
+	{
+		return m_gravity;
+	}
+
+	void			setDamping(btScalar lin_damping, btScalar ang_damping);
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+
+	btScalar getLinearSleepingThreshold() const
+	{
+		return m_linearSleepingThreshold;
+	}
+
+	btScalar getAngularSleepingThreshold() const
+	{
+		return m_angularSleepingThreshold;
+	}
+
+	void			applyDamping(btScalar timeStep);
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
+			return m_collisionShape;
+	}
+	
+	void			setMassProps(btScalar mass, const btVector3& inertia);
+	
+	btScalar		getInvMass() const { return m_inverseMass; }
+	const btMatrix3x3& getInvInertiaTensorWorld() const { 
+		return m_invInertiaTensorWorld; 
+	}
+		
+	void			integrateVelocities(btScalar step);
+
+	void			setCenterOfMassTransform(const btTransform& xform);
+
+	void			applyCentralForce(const btVector3& force)
+	{
+		m_totalForce += force;
+	}
+    
+	const btVector3& getInvInertiaDiagLocal()
+	{
+		return m_invInertiaLocal;
+	};
+
+	void	setInvInertiaDiagLocal(const btVector3& diagInvInertia)
+	{
+		m_invInertiaLocal = diagInvInertia;
+	}
+
+	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	{
+		m_linearSleepingThreshold = linear;
+		m_angularSleepingThreshold = angular;
+	}
+
+	void	applyTorque(const btVector3& torque)
+	{
+		m_totalTorque += torque;
+	}
+	
+	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
+	{
+		applyCentralForce(force);
+		applyTorque(rel_pos.cross(force)*m_angularFactor);
+	}
+	
+	void applyCentralImpulse(const btVector3& impulse)
+	{
+		m_linearVelocity += impulse * m_inverseMass;
+	}
+	
+  	void applyTorqueImpulse(const btVector3& torque)
+	{
+			m_angularVelocity += m_invInertiaTensorWorld * torque;
+	}
+	
+	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
+	{
+		if (m_inverseMass != btScalar(0.))
+		{
+			applyCentralImpulse(impulse);
+			if (m_angularFactor)
+			{
+				applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
+			}
+		}
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_inverseMass != btScalar(0.))
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			if (m_angularFactor)
+			{
+				m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+			}
+		}
+	}
+	
+	void clearForces() 
+	{
+		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+		m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	}
+	
+	void updateInertiaTensor();    
+	
+	const btPoint3&     getCenterOfMassPosition() const { 
+		return m_worldTransform.getOrigin(); 
+	}
+	btQuaternion getOrientation() const;
+	
+	const btTransform&  getCenterOfMassTransform() const { 
+		return m_worldTransform; 
+	}
+	const btVector3&   getLinearVelocity() const { 
+		return m_linearVelocity; 
+	}
+	const btVector3&    getAngularVelocity() const { 
+		return m_angularVelocity; 
+	}
+	
+
+	inline void setLinearVelocity(const btVector3& lin_vel)
+	{ 
+		assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
+		m_linearVelocity = lin_vel; 
+	}
+
+	inline void setAngularVelocity(const btVector3& ang_vel) { 
+		assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT);
+		{
+			m_angularVelocity = ang_vel; 
+		}
+	}
+
+	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
+	{
+		//we also calculate lin/ang velocity for kinematic objects
+		return m_linearVelocity + m_angularVelocity.cross(rel_pos);
+
+		//for kinematic objects, we could also use use:
+		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
+	}
+
+	void translate(const btVector3& v) 
+	{
+		m_worldTransform.getOrigin() += v; 
+	}
+
+	
+	void	getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+
+
+
+
+	
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
+	{
+		btVector3 r0 = pos - getCenterOfMassPosition();
+
+		btVector3 c0 = (r0).cross(normal);
+
+		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
+
+		return m_inverseMass + normal.dot(vec);
+
+	}
+
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	{
+		btVector3 vec = axis * getInvInertiaTensorWorld();
+		return axis.dot(vec);
+	}
+
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
+	{
+		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+			return;
+
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
+		{
+			m_deactivationTime += timeStep;
+		} else
+		{
+			m_deactivationTime=btScalar(0.);
+			setActivationState(0);
+		}
+
+	}
+
+	SIMD_FORCE_INLINE bool	wantsSleeping()
+	{
+
+		if (getActivationState() == DISABLE_DEACTIVATION)
+			return false;
+
+		//disable deactivation
+		if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+			return false;
+
+		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+			return true;
+
+		if (m_deactivationTime> gDeactivationTime)
+		{
+			return true;
+		}
+		return false;
+	}
+
+
+	
+	const btBroadphaseProxy*	getBroadphaseProxy() const
+	{
+		return m_broadphaseHandle;
+	}
+	btBroadphaseProxy*	getBroadphaseProxy() 
+	{
+		return m_broadphaseHandle;
+	}
+	void	setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
+	{
+		m_broadphaseHandle = broadphaseProxy;
+	}
+
+	//btMotionState allows to automatic synchronize the world transform for active objects
+	btMotionState*	getMotionState()
+	{
+		return m_optionalMotionState;
+	}
+	const btMotionState*	getMotionState() const
+	{
+		return m_optionalMotionState;
+	}
+	void	setMotionState(btMotionState* motionState)
+	{
+		m_optionalMotionState = motionState;
+		if (m_optionalMotionState)
+			motionState->getWorldTransform(m_worldTransform);
+	}
+
+	//for experimental overriding of friction/contact solver func
+	int	m_contactSolverType;
+	int	m_frictionSolverType;
+
+	void	setAngularFactor(btScalar angFac)
+	{
+		m_angularFactor = angFac;
+	}
+	btScalar	getAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
+	bool	isInWorld() const
+	{
+		return (getBroadphaseProxy() != 0);
+	}
+
+	virtual bool checkCollideWithOverride(btCollisionObject* co);
+
+	void addConstraintRef(btTypedConstraint* c);
+	void removeConstraintRef(btTypedConstraint* c);
+
+	btTypedConstraint* getConstraintRef(int index)
+	{
+		return m_constraintRefs[index];
+	}
+
+	int getNumConstraintRefs()
+	{
+		return m_constraintRefs.size();
+	}
+
+	int	m_debugBodyId;
+};
+
+
+
+#endif
+
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp	(revision 1963)
@@ -0,0 +1,243 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSimpleDynamicsWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C" 
+{
+	void btBulletDynamicsProbe ();
+	void btBulletDynamicsProbe () {}
+}
+
+
+
+
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
+m_ownsConstraintSolver(false),
+m_gravity(0,0,-10)
+{
+
+}
+
+
+btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
+{
+	if (m_ownsConstraintSolver)
+		btAlignedFree( m_constraintSolver);
+}
+
+int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+{
+	(void)fixedTimeStep;
+	(void)maxSubSteps;
+
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo&	dispatchInfo = getDispatchInfo();
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	///solve contact constraints
+	int numManifolds = m_dispatcher1->getNumManifolds();
+	if (numManifolds)
+	{
+		btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
+		
+		btContactSolverInfo infoGlobal;
+		infoGlobal.m_timeStep = timeStep;
+		m_constraintSolver->prepareSolve(0,numManifolds);
+		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
+	}
+
+	///integrate transforms
+	integrateTransforms(timeStep);
+		
+	updateAabbs();
+
+	synchronizeMotionStates();
+
+	clearForces();
+
+	return 1;
+
+}
+
+void	btSimpleDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
+
+void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	m_gravity = gravity;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->setGravity(gravity);
+		}
+	}
+}
+
+btVector3 btSimpleDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
+void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+	removeCollisionObject(body);
+}
+
+void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+{
+	body->setGravity(m_gravity);
+
+	if (body->getCollisionShape())
+	{
+		addCollisionObject(body);
+	}
+}
+
+void	btSimpleDynamicsWorld::updateAabbs()
+{
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (body->isActive() && (!body->isStaticObject()))
+			{
+				btPoint3 minAabb,maxAabb;
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				btBroadphaseInterface* bp = getBroadphase();
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+			}
+		}
+	}
+}
+
+void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	btTransform predictedTrans;
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (body->isActive() && (!body->isStaticObject()))
+			{
+				body->predictIntegratedTransform(timeStep, predictedTrans);
+				body->proceedToTransform( predictedTrans);
+			}
+		}
+	}
+}
+
+
+
+void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			if (!body->isStaticObject())
+			{
+				if (body->isActive())
+				{
+					body->applyGravity();
+					body->integrateVelocities( timeStep);
+					body->applyDamping(timeStep);
+					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+				}
+			}
+		}
+	}
+}
+
+
+void	btSimpleDynamicsWorld::synchronizeMotionStates()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->getMotionState())
+		{
+			if (body->getActivationState() != ISLAND_SLEEPING)
+			{
+				body->getMotionState()->setWorldTransform(body->getWorldTransform());
+			}
+		}
+	}
+
+}
+
+
+void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+	if (m_ownsConstraintSolver)
+	{
+		btAlignedFree(m_constraintSolver);
+	}
+	m_ownsConstraintSolver = false;
+	m_constraintSolver = solver;
+}
+
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
Index: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h	(revision 1963)
@@ -0,0 +1,78 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SIMPLE_DYNAMICS_WORLD_H
+#define BT_SIMPLE_DYNAMICS_WORLD_H
+
+#include "btDynamicsWorld.h"
+
+class btDispatcher;
+class btOverlappingPairCache;
+class btConstraintSolver;
+
+///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
+class btSimpleDynamicsWorld : public btDynamicsWorld
+{
+protected:
+
+	btConstraintSolver*	m_constraintSolver;
+
+	bool	m_ownsConstraintSolver;
+
+	void	predictUnconstraintMotion(btScalar timeStep);
+	
+	void	integrateTransforms(btScalar timeStep);
+		
+	btVector3	m_gravity;
+	
+public:
+
+
+
+	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
+	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btSimpleDynamicsWorld();
+		
+	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
+	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+
+	virtual void	setGravity(const btVector3& gravity);
+
+	virtual btVector3 getGravity () const;
+
+	virtual void	addRigidBody(btRigidBody* body);
+
+	virtual void	removeRigidBody(btRigidBody* body);
+	
+	virtual void	updateAabbs();
+
+	void	synchronizeMotionStates();
+
+	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_SIMPLE_DYNAMICS_WORLD;
+	}
+
+	virtual void	clearForces();
+
+};
+
+#endif //BT_SIMPLE_DYNAMICS_WORLD_H
Index: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp	(revision 1963)
@@ -0,0 +1,729 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "LinearMath/btVector3.h"
+#include "btRaycastVehicle.h"
+
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
+#include "LinearMath/btQuaternion.h"
+#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
+#include "btVehicleRaycaster.h"
+#include "btWheelInfo.h"
+#include "LinearMath/btMinMax.h"
+
+
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+static btRigidBody s_fixedObject( 0,0,0);
+
+btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
+: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
+m_vehicleRaycaster(raycaster),
+m_pitchControl(btScalar(0.))
+{
+	m_chassisBody = chassis;
+	m_indexRightAxis = 0;
+	m_indexUpAxis = 2;
+	m_indexForwardAxis = 1;
+	defaultInit(tuning);
+}
+
+
+void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
+{
+	(void)tuning;
+	m_currentVehicleSpeedKmHour = btScalar(0.);
+	m_steeringValue = btScalar(0.);
+	
+}
+
+	
+
+btRaycastVehicle::~btRaycastVehicle()
+{
+}
+
+
+//
+// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
+//
+btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
+{
+
+	btWheelInfoConstructionInfo ci;
+
+	ci.m_chassisConnectionCS = connectionPointCS;
+	ci.m_wheelDirectionCS = wheelDirectionCS0;
+	ci.m_wheelAxleCS = wheelAxleCS;
+	ci.m_suspensionRestLength = suspensionRestLength;
+	ci.m_wheelRadius = wheelRadius;
+	ci.m_suspensionStiffness = tuning.m_suspensionStiffness;
+	ci.m_wheelsDampingCompression = tuning.m_suspensionCompression;
+	ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping;
+	ci.m_frictionSlip = tuning.m_frictionSlip;
+	ci.m_bIsFrontWheel = isFrontWheel;
+	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
+
+	m_wheelInfo.push_back( btWheelInfo(ci));
+	
+	btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
+	
+	updateWheelTransformsWS( wheel , false );
+	updateWheelTransform(getNumWheels()-1,false);
+	return wheel;
+}
+
+
+
+
+const btTransform&	btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
+{
+	assert(wheelIndex < getNumWheels());
+	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
+	return wheel.m_worldTransform;
+
+}
+
+void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
+{
+	
+	btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
+	updateWheelTransformsWS(wheel,interpolatedTransform);
+	btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
+	const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
+	btVector3 fwd = up.cross(right);
+	fwd = fwd.normalize();
+//	up = right.cross(fwd);
+//	up.normalize();
+
+	//rotate around steering over de wheelAxleWS
+	btScalar steering = wheel.m_steering;
+	
+	btQuaternion steeringOrn(up,steering);//wheel.m_steering);
+	btMatrix3x3 steeringMat(steeringOrn);
+
+	btQuaternion rotatingOrn(right,-wheel.m_rotation);
+	btMatrix3x3 rotatingMat(rotatingOrn);
+
+	btMatrix3x3 basis2(
+		right[0],fwd[0],up[0],
+		right[1],fwd[1],up[1],
+		right[2],fwd[2],up[2]
+	);
+	
+	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
+	wheel.m_worldTransform.setOrigin(
+		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
+	);
+}
+
+void btRaycastVehicle::resetSuspension()
+{
+
+	int i;
+	for (i=0;i<m_wheelInfo.size();	i++)
+	{
+			btWheelInfo& wheel = m_wheelInfo[i];
+			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+			
+			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+			//wheel_info.setContactFriction(btScalar(0.0));
+			wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+	}
+}
+
+void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
+{
+	wheel.m_raycastInfo.m_isInContact = false;
+
+	btTransform chassisTrans = getChassisWorldTransform();
+	if (interpolatedTransform && (getRigidBody()->getMotionState()))
+	{
+		getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
+	}
+
+	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
+	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
+	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
+}
+
+btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
+{
+	updateWheelTransformsWS( wheel,false);
+
+	
+	btScalar depth = -1;
+	
+	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
+
+	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
+	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
+	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
+	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
+
+	btScalar param = btScalar(0.);
+	
+	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
+
+	assert(m_vehicleRaycaster);
+
+	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
+
+	wheel.m_raycastInfo.m_groundObject = 0;
+
+	if (object)
+	{
+		param = rayResults.m_distFraction;
+		depth = raylen * rayResults.m_distFraction;
+		wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
+		wheel.m_raycastInfo.m_isInContact = true;
+		
+		wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!;
+		//wheel.m_raycastInfo.m_groundObject = object;
+
+
+		btScalar hitDistance = param*raylen;
+		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
+		//clamp on max suspension travel
+
+		btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+		btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
+		{
+			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
+		}
+		if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
+		{
+			wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
+		}
+
+		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
+
+		btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
+
+		btVector3 chassis_velocity_at_contactPoint;
+		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
+
+		chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
+
+		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+
+		if ( denominator >= btScalar(-0.1))
+		{
+			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+			wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
+		}
+		else
+		{
+			btScalar inv = btScalar(-1.) / denominator;
+			wheel.m_suspensionRelativeVelocity = projVel * inv;
+			wheel.m_clippedInvContactDotSuspension = inv;
+		}
+			
+	} else
+	{
+		//put wheel info as in rest position
+		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+	}
+
+	return depth;
+}
+
+
+const btTransform& btRaycastVehicle::getChassisWorldTransform() const
+{
+	/*if (getRigidBody()->getMotionState())
+	{
+		btTransform chassisWorldTrans;
+		getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
+		return chassisWorldTrans;
+	}
+	*/
+
+	
+	return getRigidBody()->getCenterOfMassTransform();
+}
+
+
+void btRaycastVehicle::updateVehicle( btScalar step )
+{
+	{
+		for (int i=0;i<getNumWheels();i++)
+		{
+			updateWheelTransform(i,false);
+		}
+	}
+
+
+	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
+	
+	const btTransform& chassisTrans = getChassisWorldTransform();
+
+	btVector3 forwardW (
+		chassisTrans.getBasis()[0][m_indexForwardAxis],
+		chassisTrans.getBasis()[1][m_indexForwardAxis],
+		chassisTrans.getBasis()[2][m_indexForwardAxis]);
+
+	if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
+	{
+		m_currentVehicleSpeedKmHour *= btScalar(-1.);
+	}
+
+	//
+	// simulate suspension
+	//
+	
+	int i=0;
+	for (i=0;i<m_wheelInfo.size();i++)
+	{
+		btScalar depth; 
+		depth = rayCast( m_wheelInfo[i]);
+	}
+
+	updateSuspension(step);
+
+	
+	for (i=0;i<m_wheelInfo.size();i++)
+	{
+		//apply suspension force
+		btWheelInfo& wheel = m_wheelInfo[i];
+		
+		btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
+		
+		btScalar gMaxSuspensionForce = btScalar(6000.);
+		if (suspensionForce > gMaxSuspensionForce)
+		{
+			suspensionForce = gMaxSuspensionForce;
+		}
+		btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
+		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
+		
+		getRigidBody()->applyImpulse(impulse, relpos);
+	
+	}
+	
+
+	
+	updateFriction( step);
+
+	
+	for (i=0;i<m_wheelInfo.size();i++)
+	{
+		btWheelInfo& wheel = m_wheelInfo[i];
+		btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
+		btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
+
+		if (wheel.m_raycastInfo.m_isInContact)
+		{
+			const btTransform&	chassisWorldTransform = getChassisWorldTransform();
+
+			btVector3 fwd (
+				chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
+				chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
+				chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
+
+			btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
+			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
+
+			btScalar proj2 = fwd.dot(vel);
+			
+			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
+			wheel.m_rotation += wheel.m_deltaRotation;
+
+		} else
+		{
+			wheel.m_rotation += wheel.m_deltaRotation;
+		}
+		
+		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
+
+	}
+
+
+
+}
+
+
+void	btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
+{
+	assert(wheel>=0 && wheel < getNumWheels());
+
+	btWheelInfo& wheelInfo = getWheelInfo(wheel);
+	wheelInfo.m_steering = steering;
+}
+
+
+
+btScalar	btRaycastVehicle::getSteeringValue(int wheel) const
+{
+	return getWheelInfo(wheel).m_steering;
+}
+
+
+void	btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
+{
+	assert(wheel>=0 && wheel < getNumWheels());
+	btWheelInfo& wheelInfo = getWheelInfo(wheel);
+	wheelInfo.m_engineForce = force;
+}
+
+
+const btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) const
+{
+	btAssert((index >= 0) && (index < 	getNumWheels()));
+	
+	return m_wheelInfo[index];
+}
+
+btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) 
+{
+	btAssert((index >= 0) && (index < 	getNumWheels()));
+	
+	return m_wheelInfo[index];
+}
+
+void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
+{
+	btAssert((wheelIndex >= 0) && (wheelIndex < 	getNumWheels()));
+	getWheelInfo(wheelIndex).m_brake = brake;
+}
+
+
+void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
+{
+	(void)deltaTime;
+
+	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
+	
+	for (int w_it=0; w_it<getNumWheels(); w_it++)
+	{
+		btWheelInfo &wheel_info = m_wheelInfo[w_it];
+		
+		if ( wheel_info.m_raycastInfo.m_isInContact )
+		{
+			btScalar force;
+			//	Spring
+			{
+				btScalar	susp_length			= wheel_info.getSuspensionRestLength();
+				btScalar	current_length = wheel_info.m_raycastInfo.m_suspensionLength;
+
+				btScalar length_diff = (susp_length - current_length);
+
+				force = wheel_info.m_suspensionStiffness
+					* length_diff * wheel_info.m_clippedInvContactDotSuspension;
+			}
+		
+			// Damper
+			{
+				btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
+				{
+					btScalar	susp_damping;
+					if ( projected_rel_vel < btScalar(0.0) )
+					{
+						susp_damping = wheel_info.m_wheelsDampingCompression;
+					}
+					else
+					{
+						susp_damping = wheel_info.m_wheelsDampingRelaxation;
+					}
+					force -= susp_damping * projected_rel_vel;
+				}
+			}
+
+			// RESULT
+			wheel_info.m_wheelsSuspensionForce = force * chassisMass;
+			if (wheel_info.m_wheelsSuspensionForce < btScalar(0.))
+			{
+				wheel_info.m_wheelsSuspensionForce = btScalar(0.);
+			}
+		}
+		else
+		{
+			wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
+		}
+	}
+
+}
+
+
+struct btWheelContactPoint
+{
+	btRigidBody* m_body0;
+	btRigidBody* m_body1;
+	btVector3	m_frictionPositionWorld;
+	btVector3	m_frictionDirectionWorld;
+	btScalar	m_jacDiagABInv;
+	btScalar	m_maxImpulse;
+
+
+	btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
+		:m_body0(body0),
+		m_body1(body1),
+		m_frictionPositionWorld(frictionPosWorld),
+		m_frictionDirectionWorld(frictionDirectionWorld),
+		m_maxImpulse(maxImpulse)
+	{
+		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
+		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
+		btScalar	relaxation = 1.f;
+		m_jacDiagABInv = relaxation/(denom0+denom1);
+	}
+
+
+
+};
+
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+{
+
+	btScalar j1=0.f;
+
+	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
+
+	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
+	
+	btScalar maxImpulse  = contactPoint.m_maxImpulse;
+	
+	btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+
+	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
+
+	// calculate j that moves us to zero relative velocity
+	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	btSetMin(j1, maxImpulse);
+	btSetMax(j1, -maxImpulse);
+
+	return j1;
+}
+
+
+
+
+btScalar sideFrictionStiffness2 = btScalar(1.0);
+void	btRaycastVehicle::updateFriction(btScalar	timeStep)
+{
+
+		//calculate the impulse, so that the wheels don't move sidewards
+		int numWheel = getNumWheels();
+		if (!numWheel)
+			return;
+
+		m_forwardWS.resize(numWheel);
+		m_axle.resize(numWheel);
+		m_forwardImpulse.resize(numWheel);
+		m_sideImpulse.resize(numWheel);
+		
+		int numWheelsOnGround = 0;
+	
+
+		//collapse all those loops into one!
+		for (int i=0;i<getNumWheels();i++)
+		{
+			btWheelInfo& wheelInfo = m_wheelInfo[i];
+			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+			if (groundObject)
+				numWheelsOnGround++;
+			m_sideImpulse[i] = btScalar(0.);
+			m_forwardImpulse[i] = btScalar(0.);
+
+		}
+	
+		{
+	
+			for (int i=0;i<getNumWheels();i++)
+			{
+
+				btWheelInfo& wheelInfo = m_wheelInfo[i];
+					
+				class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+
+				if (groundObject)
+				{
+
+					const btTransform& wheelTrans = getWheelTransformWS( i );
+
+					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
+					m_axle[i] = btVector3(	
+						wheelBasis0[0][m_indexRightAxis],
+						wheelBasis0[1][m_indexRightAxis],
+						wheelBasis0[2][m_indexRightAxis]);
+					
+					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
+					btScalar proj = m_axle[i].dot(surfNormalWS);
+					m_axle[i] -= surfNormalWS * proj;
+					m_axle[i] = m_axle[i].normalize();
+					
+					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+					m_forwardWS[i].normalize();
+
+				
+					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
+							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
+							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
+
+					m_sideImpulse[i] *= sideFrictionStiffness2;
+						
+				}
+				
+
+			}
+		}
+
+	btScalar sideFactor = btScalar(1.);
+	btScalar fwdFactor = 0.5;
+
+	bool sliding = false;
+	{
+		for (int wheel =0;wheel <getNumWheels();wheel++)
+		{
+			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+
+			btScalar	rollingFriction = 0.f;
+
+			if (groundObject)
+			{
+				if (wheelInfo.m_engineForce != 0.f)
+				{
+					rollingFriction = wheelInfo.m_engineForce* timeStep;
+				} else
+				{
+					btScalar defaultRollingFrictionImpulse = 0.f;
+					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
+					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
+					rollingFriction = calcRollingFriction(contactPt);
+				}
+			}
+
+			//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+			
+
+
+
+			m_forwardImpulse[wheel] = btScalar(0.);
+			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+
+			if (groundObject)
+			{
+				m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+				
+				btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
+				btScalar maximpSide = maximp;
+
+				btScalar maximpSquared = maximp * maximpSide;
+			
+
+				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+
+				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
+				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
+				
+				btScalar impulseSquared = (x*x + y*y);
+
+				if (impulseSquared > maximpSquared)
+				{
+					sliding = true;
+					
+					btScalar factor = maximp / btSqrt(impulseSquared);
+					
+					m_wheelInfo[wheel].m_skidInfo *= factor;
+				}
+			} 
+
+		}
+	}
+
+	
+
+
+		if (sliding)
+		{
+			for (int wheel = 0;wheel < getNumWheels(); wheel++)
+			{
+				if (m_sideImpulse[wheel] != btScalar(0.))
+				{
+					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
+					{
+						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
+						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+					}
+				}
+			}
+		}
+
+		// apply the impulses
+		{
+			for (int wheel = 0;wheel<getNumWheels() ; wheel++)
+			{
+				btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+
+				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
+						m_chassisBody->getCenterOfMassPosition();
+
+				if (m_forwardImpulse[wheel] != btScalar(0.))
+				{
+					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
+				}
+				if (m_sideImpulse[wheel] != btScalar(0.))
+				{
+					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
+
+					btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
+						groundObject->getCenterOfMassPosition();
+
+					
+					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
+
+					rel_pos[2] *= wheelInfo.m_rollInfluence;
+					m_chassisBody->applyImpulse(sideImp,rel_pos);
+
+					//apply friction impulse on the ground
+					groundObject->applyImpulse(-sideImp,rel_pos2);
+				}
+			}
+		}
+
+	
+}
+
+
+void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
+{
+//	RayResultCallback& resultCallback;
+
+	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
+
+	m_dynamicsWorld->rayTest(from, to, rayCallback);
+
+	if (rayCallback.hasHit())
+	{
+		
+		btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
+        if (body && body->hasContactResponse())
+		{
+			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
+			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
+			result.m_hitNormalInWorld.normalize();
+			result.m_distFraction = rayCallback.m_closestHitFraction;
+			return body;
+		}
+	}
+	return 0;
+}
Index: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h	(revision 1963)
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef RAYCASTVEHICLE_H
+#define RAYCASTVEHICLE_H
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "btVehicleRaycaster.h"
+class btDynamicsWorld;
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btWheelInfo.h"
+
+class btVehicleTuning;
+
+///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
+class btRaycastVehicle : public btTypedConstraint
+{
+
+		btAlignedObjectArray<btVector3>	m_forwardWS;
+		btAlignedObjectArray<btVector3>	m_axle;
+		btAlignedObjectArray<btScalar>	m_forwardImpulse;
+		btAlignedObjectArray<btScalar>	m_sideImpulse;
+
+public:
+	class btVehicleTuning
+		{
+			public:
+
+			btVehicleTuning()
+				:m_suspensionStiffness(btScalar(5.88)),
+				m_suspensionCompression(btScalar(0.83)),
+				m_suspensionDamping(btScalar(0.88)),
+				m_maxSuspensionTravelCm(btScalar(500.)),
+				m_frictionSlip(btScalar(10.5))
+			{
+			}
+			btScalar	m_suspensionStiffness;
+			btScalar	m_suspensionCompression;
+			btScalar	m_suspensionDamping;
+			btScalar	m_maxSuspensionTravelCm;
+			btScalar	m_frictionSlip;
+
+		};
+private:
+
+	btScalar	m_tau;
+	btScalar	m_damping;
+	btVehicleRaycaster*	m_vehicleRaycaster;
+	btScalar		m_pitchControl;
+	btScalar	m_steeringValue; 
+	btScalar m_currentVehicleSpeedKmHour;
+
+	btRigidBody* m_chassisBody;
+
+	int m_indexRightAxis;
+	int m_indexUpAxis;
+	int	m_indexForwardAxis;
+
+	void defaultInit(const btVehicleTuning& tuning);
+
+public:
+
+	//constructor to create a car from an existing rigidbody
+	btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster );
+
+	virtual ~btRaycastVehicle() ;
+
+		
+	const btTransform& getChassisWorldTransform() const;
+	
+	btScalar rayCast(btWheelInfo& wheel);
+
+	virtual void updateVehicle(btScalar step);
+
+	void resetSuspension();
+
+	btScalar	getSteeringValue(int wheel) const;
+
+	void	setSteeringValue(btScalar steering,int wheel);
+
+
+	void	applyEngineForce(btScalar force, int wheel);
+
+	const btTransform&	getWheelTransformWS( int wheelIndex ) const;
+
+	void	updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
+	
+	void	setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
+
+	btWheelInfo&	addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
+
+	inline int		getNumWheels() const {
+		return int (m_wheelInfo.size());
+	}
+	
+	btAlignedObjectArray<btWheelInfo>	m_wheelInfo;
+
+
+	const btWheelInfo&	getWheelInfo(int index) const;
+
+	btWheelInfo&	getWheelInfo(int index);
+
+	void	updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
+
+	
+	void setBrake(btScalar brake,int wheelIndex);
+
+	void	setPitchControl(btScalar pitch)
+	{
+		m_pitchControl = pitch;
+	}
+	
+	void	updateSuspension(btScalar deltaTime);
+
+	virtual void	updateFriction(btScalar	timeStep);
+
+
+
+	inline btRigidBody* getRigidBody()
+	{
+		return m_chassisBody;
+	}
+
+	const btRigidBody* getRigidBody() const
+	{
+		return m_chassisBody;
+	}
+
+	inline int	getRightAxis() const
+	{
+		return m_indexRightAxis;
+	}
+	inline int getUpAxis() const
+	{
+		return m_indexUpAxis;
+	}
+
+	inline int getForwardAxis() const
+	{
+		return m_indexForwardAxis;
+	}
+
+	
+	///Worldspace forward vector
+	btVector3 getForwardVector() const
+	{
+		const btTransform& chassisTrans = getChassisWorldTransform(); 
+
+		btVector3 forwardW ( 
+			  chassisTrans.getBasis()[0][m_indexForwardAxis], 
+			  chassisTrans.getBasis()[1][m_indexForwardAxis], 
+			  chassisTrans.getBasis()[2][m_indexForwardAxis]); 
+
+		return forwardW;
+	}
+
+	///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
+	btScalar	getCurrentSpeedKmHour() const
+	{
+		return m_currentVehicleSpeedKmHour;
+	}
+
+	virtual void	setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
+	{
+		m_indexRightAxis = rightIndex;
+		m_indexUpAxis = upIndex;
+		m_indexForwardAxis = forwardIndex;
+	}
+
+	virtual void	buildJacobian()
+	{
+		//not yet
+	}
+
+	virtual	void	solveConstraint(btScalar	timeStep)
+	{
+		(void)timeStep;
+		//not yet
+	}
+
+
+};
+
+class btDefaultVehicleRaycaster : public btVehicleRaycaster
+{
+	btDynamicsWorld*	m_dynamicsWorld;
+public:
+	btDefaultVehicleRaycaster(btDynamicsWorld* world)
+		:m_dynamicsWorld(world)
+	{
+	}
+
+	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
+
+};
+
+
+#endif //RAYCASTVEHICLE_H
+
Index: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Vehicle/btVehicleRaycaster.h	(revision 1963)
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef VEHICLE_RAYCASTER_H
+#define VEHICLE_RAYCASTER_H
+
+#include "LinearMath/btVector3.h"
+
+/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
+struct btVehicleRaycaster
+{
+virtual ~btVehicleRaycaster()
+{
+}
+	struct btVehicleRaycasterResult
+	{
+		btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
+		btVector3	m_hitPointInWorld;
+		btVector3	m_hitNormalInWorld;
+		btScalar	m_distFraction;
+	};
+
+	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
+
+};
+
+#endif //VEHICLE_RAYCASTER_H
+
Index: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.cpp	(revision 1963)
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#include "btWheelInfo.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
+
+
+btScalar btWheelInfo::getSuspensionRestLength() const
+{
+
+	return m_suspensionRestLength1;
+
+}
+
+void	btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
+{
+	(void)raycastInfo;
+
+	
+	if (m_raycastInfo.m_isInContact)
+
+	{
+		btScalar	project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
+		btVector3	 chassis_velocity_at_contactPoint;
+		btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
+		chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
+		btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+		if ( project >= btScalar(-0.1))
+		{
+			m_suspensionRelativeVelocity = btScalar(0.0);
+			m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
+		}
+		else
+		{
+			btScalar inv = btScalar(-1.) / project;
+			m_suspensionRelativeVelocity = projVel * inv;
+			m_clippedInvContactDotSuspension = inv;
+		}
+		
+	}
+
+	else	// Not in contact : position wheel in a nice (rest length) position
+	{
+		m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
+		m_suspensionRelativeVelocity = btScalar(0.0);
+		m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
+		m_clippedInvContactDotSuspension = btScalar(1.0);
+	}
+}
Index: code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.h
===================================================================
--- code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletDynamics/Vehicle/btWheelInfo.h	(revision 1963)
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef WHEEL_INFO_H
+#define WHEEL_INFO_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+
+class btRigidBody;
+
+struct btWheelInfoConstructionInfo
+{
+	btVector3	m_chassisConnectionCS;
+	btVector3	m_wheelDirectionCS;
+	btVector3	m_wheelAxleCS;
+	btScalar	m_suspensionRestLength;
+	btScalar	m_maxSuspensionTravelCm;
+	btScalar	m_wheelRadius;
+	
+	btScalar		m_suspensionStiffness;
+	btScalar		m_wheelsDampingCompression;
+	btScalar		m_wheelsDampingRelaxation;
+	btScalar		m_frictionSlip;
+	bool m_bIsFrontWheel;
+	
+};
+
+/// btWheelInfo contains information per wheel about friction and suspension.
+struct btWheelInfo
+{
+	struct RaycastInfo
+	{
+		//set by raycaster
+		btVector3	m_contactNormalWS;//contactnormal
+		btVector3	m_contactPointWS;//raycast hitpoint
+		btScalar	m_suspensionLength;
+		btVector3	m_hardPointWS;//raycast starting point
+		btVector3	m_wheelDirectionWS; //direction in worldspace
+		btVector3	m_wheelAxleWS; // axle in worldspace
+		bool		m_isInContact;
+		void*		m_groundObject; //could be general void* ptr
+	};
+
+	RaycastInfo	m_raycastInfo;
+
+	btTransform	m_worldTransform;
+	
+	btVector3	m_chassisConnectionPointCS; //const
+	btVector3	m_wheelDirectionCS;//const
+	btVector3	m_wheelAxleCS; // const or modified by steering
+	btScalar	m_suspensionRestLength1;//const
+	btScalar	m_maxSuspensionTravelCm;
+	btScalar getSuspensionRestLength() const;
+	btScalar	m_wheelsRadius;//const
+	btScalar	m_suspensionStiffness;//const
+	btScalar	m_wheelsDampingCompression;//const
+	btScalar	m_wheelsDampingRelaxation;//const
+	btScalar	m_frictionSlip;
+	btScalar	m_steering;
+	btScalar	m_rotation;
+	btScalar	m_deltaRotation;
+	btScalar	m_rollInfluence;
+
+	btScalar	m_engineForce;
+
+	btScalar	m_brake;
+	
+	bool m_bIsFrontWheel;
+	
+	void*		m_clientInfo;//can be used to store pointer to sync transforms...
+
+	btWheelInfo(btWheelInfoConstructionInfo& ci)
+
+	{
+
+		m_suspensionRestLength1 = ci.m_suspensionRestLength;
+		m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
+
+		m_wheelsRadius = ci.m_wheelRadius;
+		m_suspensionStiffness = ci.m_suspensionStiffness;
+		m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
+		m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
+		m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
+		m_wheelDirectionCS = ci.m_wheelDirectionCS;
+		m_wheelAxleCS = ci.m_wheelAxleCS;
+		m_frictionSlip = ci.m_frictionSlip;
+		m_steering = btScalar(0.);
+		m_engineForce = btScalar(0.);
+		m_rotation = btScalar(0.);
+		m_deltaRotation = btScalar(0.);
+		m_brake = btScalar(0.);
+		m_rollInfluence = btScalar(0.1);
+		m_bIsFrontWheel = ci.m_bIsFrontWheel;
+
+	}
+
+	void	updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
+
+	btScalar	m_clippedInvContactDotSuspension;
+	btScalar	m_suspensionRelativeVelocity;
+	//calculated by suspension
+	btScalar	m_wheelsSuspensionForce;
+	btScalar	m_skidInfo;
+
+};
+
+#endif //WHEEL_INFO_H
+
Index: code/branches/physics/src/bullet/BulletLicense.txt
===================================================================
--- code/branches/physics/src/bullet/BulletLicense.txt	(revision 1963)
+++ code/branches/physics/src/bullet/BulletLicense.txt	(revision 1963)
@@ -0,0 +1,17 @@
+/*
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+Free for commercial use, but please mail bullet@erwincoumans.com to report projects, and join the forum at
+www.continuousphysics.com/Bullet/phpBB2
Index: code/branches/physics/src/bullet/BulletMultiThreaded/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,12 @@
+ADD_LIBRARY(LibBulletMultiThreaded
+		PlatformDefinitions.h
+		btThreadSupportInterface.cpp
+		btThreadSupportInterface.h
+		
+		Win32ThreadSupport.cpp
+		Win32ThreadSupport.h
+		PosixThreadSupport.cpp
+		PosixThreadSupport.h
+		SequentialThreadSupport.cpp
+		SequentialThreadSupport.h
+)
Index: code/branches/physics/src/bullet/BulletMultiThreaded/PlatformDefinitions.h
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/PlatformDefinitions.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/PlatformDefinitions.h	(revision 1963)
@@ -0,0 +1,82 @@
+#ifndef TYPE_DEFINITIONS_H
+#define TYPE_DEFINITIONS_H
+
+///This file provides some platform/compiler checks for common definitions
+
+#ifdef WIN32
+
+typedef union
+{
+  unsigned int u;
+  void *p;
+} addr64;
+
+#define USE_WIN32_THREADING 1
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+		#else
+		#endif //__MINGW32__
+
+		typedef unsigned char     uint8_t;
+#ifndef __PHYSICS_COMMON_H__
+		typedef unsigned long int uint64_t;
+		typedef unsigned int      uint32_t;
+#endif //__PHYSICS_COMMON_H__
+		typedef unsigned short    uint16_t;
+
+		#include <malloc.h>
+		#define memalign(alignment, size) malloc(size);
+			
+#include <string.h> //memcpy
+
+		
+
+		#include <stdio.h>		
+		#define spu_printf printf
+		
+#else
+		#include <stdint.h>
+		#include <stdlib.h>
+		#include <string.h> //for memcpy
+
+#if defined	(__CELLOS_LV2__)
+	// Playstation 3 Cell SDK
+#include <spu_printf.h>
+		
+#else
+	// posix system
+
+#define USE_PTHREADS    (1)
+
+#ifdef USE_LIBSPE2
+#include <stdio.h>		
+#define spu_printf printf	
+#define DWORD unsigned int
+		
+			typedef union
+			{
+			  unsigned long long ull;
+			  unsigned int ui[2];
+			  void *p;
+			} addr64;
+		
+		
+#else
+
+#include <stdio.h>		
+#define spu_printf printf	
+
+#endif // USE_LIBSPE2
+	
+#endif	//__CELLOS_LV2__
+	
+#endif
+
+
+/* Included here because we need uint*_t typedefs */
+#include "PpuAddressSpace.h"
+
+#endif //TYPE_DEFINITIONS_H
+
+
+
Index: code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.cpp	(revision 1963)
@@ -0,0 +1,211 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <stdio.h>
+#include "PosixThreadSupport.h"
+
+
+#ifdef USE_PTHREADS
+
+#include "SpuCollisionTaskProcess.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+#define checkPThreadFunction(returnValue) \
+    if(0 != returnValue) { \
+        printf("PThread problem at line %i in file %s: %i\n", __LINE__, __FILE__, returnValue); \
+    }
+
+// The number of threads should be equal to the number of available cores
+// Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
+
+// PosixThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+// Setup and initialize SPU/CELL/Libspe2
+PosixThreadSupport::PosixThreadSupport(ThreadConstructionInfo& threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+// cleanup/shutdown Libspe2
+PosixThreadSupport::~PosixThreadSupport()
+{
+	stopSPU();
+}
+
+// this semaphore will signal, if and how many threads are finished with their work
+static sem_t mainSemaphore;
+
+static void *threadFunction(void *argument) 
+{
+
+	PosixThreadSupport::btSpuStatus* status = (PosixThreadSupport::btSpuStatus*)argument;
+
+	
+	while (1)
+	{
+            checkPThreadFunction(sem_wait(&status->startSemaphore));
+		
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr,status->m_lsMemory);
+			status->m_status = 2;
+			checkPThreadFunction(sem_post(&mainSemaphore));
+
+            status->threadUsed++;
+		} else {
+			//exit Thread
+			status->m_status = 3;
+			checkPThreadFunction(sem_post(&mainSemaphore));
+			printf("Thread with taskId %i exiting\n",status->m_taskId);
+			break;
+		}
+		
+	}
+
+	printf("Thread TERMINATED\n");
+	return 0;
+
+}
+
+///send messages to SPUs
+void PosixThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	///	gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc);
+	
+	///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
+	
+
+
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskId];
+			btAssert(taskId >= 0);
+			btAssert(taskId < m_activeSpuStatus.size());
+
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = 1;
+			spuStatus.m_userPtr = (void*)uiArgument0;
+
+			// fire event to start new task
+            checkPThreadFunction(sem_post(&spuStatus.startSemaphore));
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+
+}
+
+
+///check for messages from SPUs
+void PosixThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+
+
+	btAssert(m_activeSpuStatus.size());
+
+        // wait for any of the threads to finish
+        checkPThreadFunction(sem_wait(&mainSemaphore));
+        
+	// get at least one thread which has finished
+        size_t last = -1;
+        
+        for(size_t t=0; t < m_activeSpuStatus.size(); ++t) {
+            if(2 == m_activeSpuStatus[t].m_status) {
+                last = t;
+                break;
+            }
+        }
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+
+	btAssert(spuStatus.m_status > 1);
+	spuStatus.m_status = 0;
+
+	// need to find an active spu
+	btAssert(last >= 0);
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+}
+
+
+
+void PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructionInfo)
+{
+        printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads);
+	m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
+        
+        checkPThreadFunction(sem_init(&mainSemaphore, 0, 0));
+
+	for (int i=0;i < threadConstructionInfo.m_numThreads;i++)
+	{
+		printf("starting thread %d\n",i);
+
+		btSpuStatus&	spuStatus = m_activeSpuStatus[i];
+                
+                checkPThreadFunction(sem_init(&spuStatus.startSemaphore, 0, 0));
+                checkPThreadFunction(pthread_create(&spuStatus.thread, NULL, &threadFunction, (void*)&spuStatus));
+
+		spuStatus.m_userPtr=0;
+
+		spuStatus.m_taskId = i;
+		spuStatus.m_commandId = 0;
+		spuStatus.m_status = 0;
+		spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+		spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+        spuStatus.threadUsed = 0;
+
+		printf("started thread %d \n",i);
+		
+	}
+
+}
+
+void PosixThreadSupport::startSPU()
+{
+}
+
+
+///tell the task scheduler we are done with the SPU tasks
+void PosixThreadSupport::stopSPU()
+{
+	for(size_t t=0; t < m_activeSpuStatus.size(); ++t) {
+            btSpuStatus&	spuStatus = m_activeSpuStatus[t];
+        printf("%s: Thread %i used: %ld\n", __FUNCTION__, t, spuStatus.threadUsed);
+        
+        
+            checkPThreadFunction(sem_destroy(&spuStatus.startSemaphore));
+            checkPThreadFunction(pthread_cancel(spuStatus.thread));
+        }
+        checkPThreadFunction(sem_destroy(&mainSemaphore));
+
+	m_activeSpuStatus.clear();
+}
+
+#endif // USE_PTHREADS
+
Index: code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.h
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/PosixThreadSupport.h	(revision 1963)
@@ -0,0 +1,122 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "LinearMath/btScalar.h"
+#include "PlatformDefinitions.h"
+
+#ifdef USE_PTHREADS  //platform specific defines are defined in PlatformDefinitions.h
+#include <pthread.h>
+#include <semaphore.h>
+
+#ifndef POSIX_THREAD_SUPPORT_H
+#define POSIX_THREAD_SUPPORT_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btThreadSupportInterface.h"
+
+
+typedef void (*PosixThreadFunc)(void* userPtr,void* lsMemory);
+typedef void* (*PosixlsMemorySetupFunc)();
+
+// PosixThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class PosixThreadSupport : public btThreadSupportInterface 
+{
+public:
+    typedef enum sStatus {
+        STATUS_BUSY,
+        STATUS_READY,
+        STATUS_FINISHED
+    } Status;
+
+	// placeholder, until libspe2 support is there
+	struct	btSpuStatus
+	{
+		uint32_t	m_taskId;
+		uint32_t	m_commandId;
+		uint32_t	m_status;
+
+		PosixThreadFunc	m_userThreadFunc;
+		void*	m_userPtr; //for taskDesc etc
+		void*	m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
+
+                pthread_t thread;
+                sem_t startSemaphore;
+
+        unsigned long threadUsed;
+	};
+private:
+
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+public:
+	///Setup and initialize SPU/CELL/Libspe2
+
+	
+
+	struct	ThreadConstructionInfo
+	{
+		ThreadConstructionInfo(char* uniqueName,
+									PosixThreadFunc userThreadFunc,
+									PosixlsMemorySetupFunc	lsMemoryFunc,
+									int numThreads=1,
+									int threadStackSize=65535
+									)
+									:m_uniqueName(uniqueName),
+									m_userThreadFunc(userThreadFunc),
+									m_lsMemoryFunc(lsMemoryFunc),
+									m_numThreads(numThreads),
+									m_threadStackSize(threadStackSize)
+		{
+
+		}
+
+		char*					m_uniqueName;
+		PosixThreadFunc			m_userThreadFunc;
+		PosixlsMemorySetupFunc	m_lsMemoryFunc;
+		int						m_numThreads;
+		int						m_threadStackSize;
+
+	};
+
+	PosixThreadSupport(ThreadConstructionInfo& threadConstructionInfo);
+
+///cleanup/shutdown Libspe2
+	virtual	~PosixThreadSupport();
+
+	void	startThreads(ThreadConstructionInfo&	threadInfo);
+
+
+///send messages to SPUs
+	virtual	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1);
+
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU();
+
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU();
+
+	virtual void setNumTasks(int numTasks)
+	{
+	}
+
+};
+
+#endif // POSIX_THREAD_SUPPORT_H
+
+#endif // USE_PTHREADS
Index: code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.cpp	(revision 1963)
@@ -0,0 +1,93 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "SequentialThreadSupport.h"
+
+
+#include "SpuCollisionTaskProcess.h"
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+///cleanup/shutdown Libspe2
+SequentialThreadSupport::~SequentialThreadSupport()
+{
+	stopSPU();
+}
+
+#include <stdio.h>
+
+///send messages to SPUs
+void SequentialThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+			btSpuStatus&	spuStatus = m_activeSpuStatus[0];
+			spuStatus.m_userPtr=(void*)uiArgument0;
+			spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
+		}
+	break;
+	default:
+		{
+			///not implemented
+			btAssert(0 && "Not implemented");
+		}
+
+	};
+
+
+}
+
+///check for messages from SPUs
+void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	btAssert(m_activeSpuStatus.size());
+	btSpuStatus& spuStatus = m_activeSpuStatus[0];
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+}
+
+void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo)
+{
+	m_activeSpuStatus.resize(1);
+	printf("STS: Not starting any threads\n");
+	btSpuStatus& spuStatus = m_activeSpuStatus[0];
+	spuStatus.m_userPtr = 0;
+	spuStatus.m_taskId = 0;
+	spuStatus.m_commandId = 0;
+	spuStatus.m_status = 0;
+	spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+	spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+	printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName);
+}
+
+void SequentialThreadSupport::startSPU()
+{
+}
+
+void SequentialThreadSupport::stopSPU()
+{
+	m_activeSpuStatus.clear();
+}
+
+void SequentialThreadSupport::setNumTasks(int numTasks)
+{
+	printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks);
+}
Index: code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.h
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/SequentialThreadSupport.h	(revision 1963)
@@ -0,0 +1,86 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "PlatformDefinitions.h"
+
+
+#ifndef SEQUENTIAL_THREAD_SUPPORT_H
+#define SEQUENTIAL_THREAD_SUPPORT_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btThreadSupportInterface.h"
+
+typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory);
+typedef void* (*SequentiallsMemorySetupFunc)();
+
+
+
+///SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface
+class SequentialThreadSupport : public btThreadSupportInterface 
+{
+public:
+	struct	btSpuStatus
+	{
+		uint32_t	m_taskId;
+		uint32_t	m_commandId;
+		uint32_t	m_status;
+
+		SequentialThreadFunc	m_userThreadFunc;
+
+		void*	m_userPtr; //for taskDesc etc
+		void*	m_lsMemory; //initialized using SequentiallsMemorySetupFunc
+	};
+private:
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+	btAlignedObjectArray<void*>			m_completeHandles;	
+public:
+	struct	SequentialThreadConstructionInfo
+	{
+		SequentialThreadConstructionInfo (char* uniqueName,
+									SequentialThreadFunc userThreadFunc,
+									SequentiallsMemorySetupFunc	lsMemoryFunc
+									)
+									:m_uniqueName(uniqueName),
+									m_userThreadFunc(userThreadFunc),
+									m_lsMemoryFunc(lsMemoryFunc)
+		{
+
+		}
+
+		char*						m_uniqueName;
+		SequentialThreadFunc		m_userThreadFunc;
+		SequentiallsMemorySetupFunc	m_lsMemoryFunc;
+	};
+
+	SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo);
+	virtual	~SequentialThreadSupport();
+	void	startThreads(SequentialThreadConstructionInfo&	threadInfo);
+///send messages to SPUs
+	virtual	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1);
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU();
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU();
+
+	virtual void setNumTasks(int numTasks);
+
+};
+
+#endif //SEQUENTIAL_THREAD_SUPPORT_H
+
Index: code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.cpp	(revision 1963)
@@ -0,0 +1,259 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Win32ThreadSupport.h"
+
+#ifdef USE_WIN32_THREADING
+
+#include <windows.h>
+
+#include "SpuCollisionTaskProcess.h"
+
+#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
+
+
+
+///The number of threads should be equal to the number of available cores
+///Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
+
+///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+///Setup and initialize SPU/CELL/Libspe2
+Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo)
+{
+	startThreads(threadConstructionInfo);
+}
+
+///cleanup/shutdown Libspe2
+Win32ThreadSupport::~Win32ThreadSupport()
+{
+	stopSPU();
+}
+
+
+
+
+#include <stdio.h>
+
+DWORD WINAPI Thread_no_1( LPVOID lpParam ) 
+{
+
+	Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam;
+
+	
+	while (1)
+	{
+		WaitForSingleObject(status->m_eventStartHandle,INFINITE);
+		
+		void* userPtr = status->m_userPtr;
+
+		if (userPtr)
+		{
+			btAssert(status->m_status);
+			status->m_userThreadFunc(userPtr,status->m_lsMemory);
+			status->m_status = 2;
+			SetEvent(status->m_eventCompletetHandle);
+		} else
+		{
+			//exit Thread
+			status->m_status = 3;
+			SetEvent(status->m_eventCompletetHandle);
+			printf("Thread with taskId %i with handle %i exiting\n",status->m_taskId, status->m_threadHandle);
+			break;
+		}
+		
+	}
+
+	printf("Thread TERMINATED\n");
+	return 0;
+
+}
+
+///send messages to SPUs
+void Win32ThreadSupport::sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t taskId)
+{
+	///	gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (uint32_t) &taskDesc);
+	
+	///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
+	
+
+
+	switch (uiCommand)
+	{
+	case 	CMD_GATHER_AND_PROCESS_PAIRLIST:
+		{
+
+
+//#define SINGLE_THREADED 1
+#ifdef SINGLE_THREADED
+
+			btSpuStatus&	spuStatus = m_activeSpuStatus[0];
+			spuStatus.m_userPtr=(void*)uiArgument0;
+			spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
+			HANDLE handle =0;
+#else
+
+
+			btSpuStatus&	spuStatus = m_activeSpuStatus[taskId];
+			btAssert(taskId>=0);
+			btAssert(taskId<m_activeSpuStatus.size());
+
+			spuStatus.m_commandId = uiCommand;
+			spuStatus.m_status = 1;
+			spuStatus.m_userPtr = (void*)uiArgument0;
+
+			///fire event to start new task
+			SetEvent(spuStatus.m_eventStartHandle);
+
+#endif //CollisionTask_LocalStoreMemory
+
+			
+
+			break;
+		}
+	default:
+		{
+			///not implemented
+			btAssert(0);
+		}
+
+	};
+
+
+}
+
+
+///check for messages from SPUs
+void Win32ThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
+{
+	///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
+	
+	///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
+
+
+	btAssert(m_activeSpuStatus.size());
+
+	int last = -1;
+#ifndef SINGLE_THREADED
+	DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE);
+	btAssert(res != WAIT_FAILED);
+	last = res - WAIT_OBJECT_0;
+
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+	btAssert(spuStatus.m_threadHandle);
+	btAssert(spuStatus.m_eventCompletetHandle);
+
+	//WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+	btAssert(spuStatus.m_status > 1);
+	spuStatus.m_status = 0;
+
+	///need to find an active spu
+	btAssert(last>=0);
+
+#else
+	last=0;
+	btSpuStatus& spuStatus = m_activeSpuStatus[last];
+#endif //SINGLE_THREADED
+
+	
+
+	*puiArgument0 = spuStatus.m_taskId;
+	*puiArgument1 = spuStatus.m_status;
+
+
+}
+
+
+
+void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
+{
+
+	m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
+	m_completeHandles.resize(threadConstructionInfo.m_numThreads);
+
+	for (int i=0;i<threadConstructionInfo.m_numThreads;i++)
+	{
+		printf("starting thread %d\n",i);
+
+		btSpuStatus&	spuStatus = m_activeSpuStatus[i];
+
+		LPSECURITY_ATTRIBUTES lpThreadAttributes=NULL;
+		SIZE_T dwStackSize=threadConstructionInfo.m_threadStackSize;
+		LPTHREAD_START_ROUTINE lpStartAddress=&Thread_no_1;
+		LPVOID lpParameter=&spuStatus;
+		DWORD dwCreationFlags=0;
+		LPDWORD lpThreadId=0;
+
+		spuStatus.m_userPtr=0;
+
+		sprintf(spuStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
+		spuStatus.m_eventStartHandle = CreateEvent(0,false,false,spuStatus.m_eventStartHandleName);
+
+		sprintf(spuStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
+		spuStatus.m_eventCompletetHandle = CreateEvent(0,false,false,spuStatus.m_eventCompletetHandleName);
+
+		m_completeHandles[i] = spuStatus.m_eventCompletetHandle;
+
+		HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter,	dwCreationFlags,lpThreadId);
+		SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
+		//SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
+
+		SetThreadAffinityMask(handle, 1<<i);
+
+		spuStatus.m_taskId = i;
+		spuStatus.m_commandId = 0;
+		spuStatus.m_status = 0;
+		spuStatus.m_threadHandle = handle;
+		spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
+		spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
+
+		printf("started thread %d with threadHandle %d\n",i,handle);
+		
+	}
+
+}
+
+void Win32ThreadSupport::startSPU()
+{
+}
+
+
+///tell the task scheduler we are done with the SPU tasks
+void Win32ThreadSupport::stopSPU()
+{
+	int i;
+	for (i=0;i<m_activeSpuStatus.size();i++)
+	{
+		btSpuStatus& spuStatus = m_activeSpuStatus[i];
+		if (spuStatus.m_status>0)
+		{
+			WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+		}
+		
+
+		spuStatus.m_userPtr = 0;
+		SetEvent(spuStatus.m_eventStartHandle);
+		WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
+
+		CloseHandle(spuStatus.m_eventCompletetHandle);
+		CloseHandle(spuStatus.m_eventStartHandle);
+		CloseHandle(spuStatus.m_threadHandle);
+	}
+
+	m_activeSpuStatus.clear();
+	m_completeHandles.clear();
+
+}
+
+#endif //USE_WIN32_THREADING
Index: code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.h
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/Win32ThreadSupport.h	(revision 1963)
@@ -0,0 +1,125 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "PlatformDefinitions.h"
+
+#ifdef USE_WIN32_THREADING  //platform specific defines are defined in PlatformDefinitions.h
+
+#ifndef WIN32_THREAD_SUPPORT_H
+#define WIN32_THREAD_SUPPORT_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "btThreadSupportInterface.h"
+
+
+typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory);
+typedef void* (*Win32lsMemorySetupFunc)();
+
+
+
+
+
+
+///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
+class Win32ThreadSupport : public btThreadSupportInterface 
+{
+public:
+	///placeholder, until libspe2 support is there
+	struct	btSpuStatus
+	{
+		uint32_t	m_taskId;
+		uint32_t	m_commandId;
+		uint32_t	m_status;
+
+		Win32ThreadFunc	m_userThreadFunc;
+		void*	m_userPtr; //for taskDesc etc
+		void*	m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc
+
+		void*	m_threadHandle; //this one is calling 'Win32ThreadFunc'
+
+		void*	m_eventStartHandle;
+		char	m_eventStartHandleName[32];
+
+		void*	m_eventCompletetHandle;
+		char	m_eventCompletetHandleName[32];
+		
+
+	};
+private:
+
+	btAlignedObjectArray<btSpuStatus>	m_activeSpuStatus;
+	btAlignedObjectArray<void*>			m_completeHandles;
+	
+public:
+	///Setup and initialize SPU/CELL/Libspe2
+
+	struct	Win32ThreadConstructionInfo
+	{
+		Win32ThreadConstructionInfo(char* uniqueName,
+									Win32ThreadFunc userThreadFunc,
+									Win32lsMemorySetupFunc	lsMemoryFunc,
+									int numThreads=1,
+									int threadStackSize=65535
+									)
+									:m_uniqueName(uniqueName),
+									m_userThreadFunc(userThreadFunc),
+									m_lsMemoryFunc(lsMemoryFunc),
+									m_numThreads(numThreads),
+									m_threadStackSize(threadStackSize)
+		{
+
+		}
+
+		char*					m_uniqueName;
+		Win32ThreadFunc			m_userThreadFunc;
+		Win32lsMemorySetupFunc	m_lsMemoryFunc;
+		int						m_numThreads;
+		int						m_threadStackSize;
+
+	};
+
+
+
+	Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo);
+
+///cleanup/shutdown Libspe2
+	virtual	~Win32ThreadSupport();
+
+	void	startThreads(const Win32ThreadConstructionInfo&	threadInfo);
+
+
+///send messages to SPUs
+	virtual	void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1);
+
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
+
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU();
+
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU();
+
+	virtual	void	setNumTasks(int numTasks)
+	{
+	}
+
+};
+
+#endif //WIN32_THREAD_SUPPORT_H
+
+#endif //USE_WIN32_THREADING
Index: code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.cpp	(revision 1963)
@@ -0,0 +1,22 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btThreadSupportInterface.h"
+
+btThreadSupportInterface::~btThreadSupportInterface()
+{
+
+}
+
Index: code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.h
===================================================================
--- code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletMultiThreaded/btThreadSupportInterface.h	(revision 1963)
@@ -0,0 +1,47 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://bulletphysics.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef THREAD_SUPPORT_INTERFACE_H
+#define THREAD_SUPPORT_INTERFACE_H
+
+
+//#include <LinearMath/btScalar.h> //for uint32_t etc.
+#include "PlatformDefinitions.h"
+
+class btThreadSupportInterface
+{
+public:
+
+	virtual ~btThreadSupportInterface();
+
+///send messages to SPUs
+	virtual void sendRequest(uint32_t uiCommand, uint32_t uiArgument0, uint32_t uiArgument1) =0;
+
+///check for messages from SPUs
+	virtual	void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1) =0;
+
+///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
+	virtual	void startSPU() =0;
+
+///tell the task scheduler we are done with the SPU tasks
+	virtual	void stopSPU()=0;
+
+	///tell the task scheduler to use no more than numTasks tasks
+	virtual void	setNumTasks(int numTasks)=0;
+
+};
+
+#endif //THREAD_SUPPORT_INTERFACE_H
+
Index: code/branches/physics/src/bullet/BulletSoftBody/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,16 @@
+ADD_LIBRARY(LibBulletSoftBody
+	btSoftBody.cpp
+	btSoftBody.h
+	btSoftBodyHelpers.cpp
+	btSparseSDF.h
+	btSoftBodyHelpers.h
+	btSoftBodyRigidBodyCollisionConfiguration.cpp
+	btSoftRigidCollisionAlgorithm.cpp
+	btSoftRigidCollisionAlgorithm.h
+	btSoftSoftCollisionAlgorithm.cpp
+	btSoftSoftCollisionAlgorithm.h
+	btSoftBodyConcaveCollisionAlgorithm.cpp
+	btSoftBodyConcaveCollisionAlgorithm.h
+	btSoftRigidDynamicsWorld.h
+	btSoftRigidDynamicsWorld.cpp
+)
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.cpp	(revision 1963)
@@ -0,0 +1,2628 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#include "btSoftBodyInternals.h"
+
+//
+btSoftBody::btSoftBody(btSoftBodyWorldInfo*	worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
+:m_worldInfo(worldInfo)
+{	
+	/* Init		*/ 
+	m_internalType		=	CO_SOFT_BODY;
+	m_cfg.aeromodel		=	eAeroModel::V_Point;
+	m_cfg.kVCF			=	1;
+	m_cfg.kDG			=	0;
+	m_cfg.kLF			=	0;
+	m_cfg.kDP			=	0;
+	m_cfg.kPR			=	0;
+	m_cfg.kVC			=	0;
+	m_cfg.kDF			=	(btScalar)0.2;
+	m_cfg.kMT			=	0;
+	m_cfg.kCHR			=	(btScalar)1.0;
+	m_cfg.kKHR			=	(btScalar)0.1;
+	m_cfg.kSHR			=	(btScalar)1.0;
+	m_cfg.kAHR			=	(btScalar)0.7;
+	m_cfg.kSRHR_CL		=	(btScalar)0.1;
+	m_cfg.kSKHR_CL		=	(btScalar)1;
+	m_cfg.kSSHR_CL		=	(btScalar)0.5;
+	m_cfg.kSR_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.kSK_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.kSS_SPLT_CL	=	(btScalar)0.5;
+	m_cfg.maxvolume		=	(btScalar)1;
+	m_cfg.timescale		=	1;
+	m_cfg.viterations	=	0;
+	m_cfg.piterations	=	1;	
+	m_cfg.diterations	=	0;
+	m_cfg.citerations	=	4;
+	m_cfg.collisions	=	fCollision::Default;
+	m_pose.m_bvolume	=	false;
+	m_pose.m_bframe		=	false;
+	m_pose.m_volume		=	0;
+	m_pose.m_com		=	btVector3(0,0,0);
+	m_pose.m_rot.setIdentity();
+	m_pose.m_scl.setIdentity();
+	m_tag				=	0;
+	m_timeacc			=	0;
+	m_bUpdateRtCst		=	true;
+	m_bounds[0]			=	btVector3(0,0,0);
+	m_bounds[1]			=	btVector3(0,0,0);
+	m_worldTransform.setIdentity();
+	setSolver(eSolverPresets::Positions);
+	/* Default material	*/ 
+	Material*	pm=appendMaterial();
+	pm->m_kLST	=	1;
+	pm->m_kAST	=	1;
+	pm->m_kVST	=	1;
+	pm->m_flags	=	fMaterial::Default;
+	/* Collision shape	*/ 
+	///for now, create a collision shape internally
+	m_collisionShape = new btSoftBodyCollisionShape(this);
+	m_collisionShape->setMargin(0.25);
+	/* Nodes			*/ 
+	const btScalar		margin=getCollisionShape()->getMargin();
+	m_nodes.resize(node_count);
+	for(int i=0,ni=node_count;i<ni;++i)
+	{	
+		Node&	n=m_nodes[i];
+		ZeroInitialize(n);
+		n.m_x		=	x?*x++:btVector3(0,0,0);
+		n.m_q		=	n.m_x;
+		n.m_im		=	m?*m++:1;
+		n.m_im		=	n.m_im>0?1/n.m_im:0;
+		n.m_leaf	=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+		n.m_material=	pm;
+	}
+	updateBounds();	
+
+
+}
+
+//
+btSoftBody::~btSoftBody()
+{
+	//for now, delete the internal shape
+	delete m_collisionShape;	
+	int i;
+
+	releaseClusters();
+	for(i=0;i<m_materials.size();++i) 
+		btAlignedFree(m_materials[i]);
+	for(i=0;i<m_joints.size();++i) 
+		btAlignedFree(m_joints[i]);
+}
+
+//
+bool			btSoftBody::checkLink(int node0,int node1) const
+{
+	return(checkLink(&m_nodes[node0],&m_nodes[node1]));
+}
+
+//
+bool			btSoftBody::checkLink(const Node* node0,const Node* node1) const
+{
+	const Node*	n[]={node0,node1};
+	for(int i=0,ni=m_links.size();i<ni;++i)
+	{
+		const Link&	l=m_links[i];
+		if(	(l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
+			(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
+		{
+			return(true);
+		}
+	}
+	return(false);
+}
+
+//
+bool			btSoftBody::checkFace(int node0,int node1,int node2) const
+{
+	const Node*	n[]={	&m_nodes[node0],
+		&m_nodes[node1],
+		&m_nodes[node2]};
+	for(int i=0,ni=m_faces.size();i<ni;++i)
+	{
+		const Face&	f=m_faces[i];
+		int			c=0;
+		for(int j=0;j<3;++j)
+		{
+			if(	(f.m_n[j]==n[0])||
+				(f.m_n[j]==n[1])||
+				(f.m_n[j]==n[2])) c|=1<<j; else break;
+		}
+		if(c==7) return(true);
+	}
+	return(false);
+}
+
+//
+btSoftBody::Material*		btSoftBody::appendMaterial()
+{
+	Material*	pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
+	if(m_materials.size()>0)
+		*pm=*m_materials[0];
+	else
+		ZeroInitialize(*pm);
+	m_materials.push_back(pm);
+	return(pm);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   const btVector4& c,
+									   Node* n0,
+									   Node* n1,
+									   Node* n2,
+									   Node* n3)
+{
+	Note	n;
+	ZeroInitialize(n);
+	n.m_rank		=	0;
+	n.m_text		=	text;
+	n.m_offset		=	o;
+	n.m_coords[0]	=	c.x();
+	n.m_coords[1]	=	c.y();
+	n.m_coords[2]	=	c.z();
+	n.m_coords[3]	=	c.w();
+	n.m_nodes[0]	=	n0;n.m_rank+=n0?1:0;
+	n.m_nodes[1]	=	n1;n.m_rank+=n1?1:0;
+	n.m_nodes[2]	=	n2;n.m_rank+=n2?1:0;
+	n.m_nodes[3]	=	n3;n.m_rank+=n3?1:0;
+	m_notes.push_back(n);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Node* feature)
+{
+	appendNote(text,o,btVector4(1,0,0,0),feature);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Link* feature)
+{
+	static const btScalar	w=1/(btScalar)2;
+	appendNote(text,o,btVector4(w,w,0,0),	feature->m_n[0],
+		feature->m_n[1]);
+}
+
+//
+void			btSoftBody::appendNote(	const char* text,
+									   const btVector3& o,
+									   Face* feature)
+{
+	static const btScalar	w=1/(btScalar)3;
+	appendNote(text,o,btVector4(w,w,w,0),	feature->m_n[0],
+		feature->m_n[1],
+		feature->m_n[2]);
+}
+
+//
+void			btSoftBody::appendNode(	const btVector3& x,btScalar m)
+{
+	if(m_nodes.capacity()==m_nodes.size())
+	{
+		pointersToIndices();
+		m_nodes.reserve(m_nodes.size()*2+1);
+		indicesToPointers();
+	}
+	const btScalar	margin=getCollisionShape()->getMargin();
+	m_nodes.push_back(Node());
+	Node&			n=m_nodes[m_nodes.size()-1];
+	ZeroInitialize(n);
+	n.m_x			=	x;
+	n.m_q			=	n.m_x;
+	n.m_im			=	m>0?1/m:0;
+	n.m_material	=	m_materials[0];
+	n.m_leaf		=	m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
+}
+
+//
+void			btSoftBody::appendLink(int model,Material* mat)
+{
+	Link	l;
+	if(model>=0)
+		l=m_links[model];
+	else
+	{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
+	m_links.push_back(l);
+}
+
+//
+void			btSoftBody::appendLink(	int node0,
+									   int node1,
+									   Material* mat,
+									   bool bcheckexist)
+{
+	appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
+}
+
+//
+void			btSoftBody::appendLink(	Node* node0,
+									   Node* node1,
+									   Material* mat,
+									   bool bcheckexist)
+{
+	if((!bcheckexist)||(!checkLink(node0,node1)))
+	{
+		appendLink(-1,mat);
+		Link&	l=m_links[m_links.size()-1];
+		l.m_n[0]		=	node0;
+		l.m_n[1]		=	node1;
+		l.m_rl			=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+		m_bUpdateRtCst=true;
+	}
+}
+
+//
+void			btSoftBody::appendFace(int model,Material* mat)
+{
+	Face	f;
+	if(model>=0)
+	{ f=m_faces[model]; }
+	else
+	{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
+	m_faces.push_back(f);
+}
+
+//
+void			btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
+{
+	if (node0==node1)
+		return;
+	if (node1==node2)
+		return;
+	if (node2==node0)
+		return;
+
+	appendFace(-1,mat);
+	Face&	f=m_faces[m_faces.size()-1];
+	btAssert(node0!=node1);
+	btAssert(node1!=node2);
+	btAssert(node2!=node0);
+	f.m_n[0]	=	&m_nodes[node0];
+	f.m_n[1]	=	&m_nodes[node1];
+	f.m_n[2]	=	&m_nodes[node2];
+	f.m_ra		=	AreaOf(	f.m_n[0]->m_x,
+		f.m_n[1]->m_x,
+		f.m_n[2]->m_x);	
+	m_bUpdateRtCst=true;
+}
+
+//
+void			btSoftBody::appendAnchor(int node,btRigidBody* body)
+{
+	Anchor	a;
+	a.m_node			=	&m_nodes[node];
+	a.m_body			=	body;
+	a.m_local			=	body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
+	a.m_node->m_battach	=	1;
+	m_anchors.push_back(a);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
+{
+	LJoint*		pj	=	new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
+	pj->m_bodies[0]	=	body0;
+	pj->m_bodies[1]	=	body1;
+	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse()*specs.position;
+	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse()*specs.position;
+	pj->m_cfm		=	specs.cfm;
+	pj->m_erp		=	specs.erp;
+	pj->m_split		=	specs.split;
+	m_joints.push_back(pj);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
+{
+	appendLinearJoint(specs,m_clusters[0],body);
+}
+
+//
+void			btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
+{
+	appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
+{
+	AJoint*		pj	=	new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
+	pj->m_bodies[0]	=	body0;
+	pj->m_bodies[1]	=	body1;
+	pj->m_refs[0]	=	pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
+	pj->m_refs[1]	=	pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
+	pj->m_cfm		=	specs.cfm;
+	pj->m_erp		=	specs.erp;
+	pj->m_split		=	specs.split;
+	pj->m_icontrol	=	specs.icontrol;
+	m_joints.push_back(pj);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
+{
+	appendAngularJoint(specs,m_clusters[0],body);
+}
+
+//
+void			btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
+{
+	appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
+}
+
+//
+void			btSoftBody::addForce(const btVector3& force)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
+}
+
+//
+void			btSoftBody::addForce(const btVector3& force,int node)
+{
+	Node&	n=m_nodes[node];
+	if(n.m_im>0)
+	{
+		n.m_f	+=	force;
+	}
+}
+
+//
+void			btSoftBody::addVelocity(const btVector3& velocity)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
+}
+
+/* Set velocity for the entire body										*/ 
+void				btSoftBody::setVelocity(	const btVector3& velocity)
+{
+	for(int i=0,ni=m_nodes.size();i<ni;++i) 
+	{
+		Node&	n=m_nodes[i];
+		if(n.m_im>0)
+		{
+			n.m_v	=	velocity;
+		}
+	}
+}
+
+
+//
+void			btSoftBody::addVelocity(const btVector3& velocity,int node)
+{
+	Node&	n=m_nodes[node];
+	if(n.m_im>0)
+	{
+		n.m_v	+=	velocity;
+	}
+}
+
+//
+void			btSoftBody::setMass(int node,btScalar mass)
+{
+	m_nodes[node].m_im=mass>0?1/mass:0;
+	m_bUpdateRtCst=true;
+}
+
+//
+btScalar		btSoftBody::getMass(int node) const
+{
+	return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
+}
+
+//
+btScalar		btSoftBody::getTotalMass() const
+{
+	btScalar	mass=0;
+	for(int i=0;i<m_nodes.size();++i)
+	{
+		mass+=getMass(i);
+	}
+	return(mass);
+}
+
+//
+void			btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
+{
+	int i;
+
+	if(fromfaces)
+	{
+
+		for(i=0;i<m_nodes.size();++i)
+		{
+			m_nodes[i].m_im=0;
+		}
+		for(i=0;i<m_faces.size();++i)
+		{
+			const Face&		f=m_faces[i];
+			const btScalar	twicearea=AreaOf(	f.m_n[0]->m_x,
+				f.m_n[1]->m_x,
+				f.m_n[2]->m_x);
+			for(int j=0;j<3;++j)
+			{
+				f.m_n[j]->m_im+=twicearea;
+			}
+		}
+		for( i=0;i<m_nodes.size();++i)
+		{
+			m_nodes[i].m_im=1/m_nodes[i].m_im;
+		}
+	}
+	const btScalar	tm=getTotalMass();
+	const btScalar	itm=1/tm;
+	for( i=0;i<m_nodes.size();++i)
+	{
+		m_nodes[i].m_im/=itm*mass;
+	}
+	m_bUpdateRtCst=true;
+}
+
+//
+void			btSoftBody::setTotalDensity(btScalar density)
+{
+	setTotalMass(getVolume()*density,true);
+}
+
+//
+void			btSoftBody::transform(const btTransform& trs)
+{
+	const btScalar	margin=getCollisionShape()->getMargin();
+	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		n.m_x=trs*n.m_x;
+		n.m_q=trs*n.m_q;
+		n.m_n=trs.getBasis()*n.m_n;
+		m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
+	}
+	updateNormals();
+	updateBounds();
+	updateConstants();
+}
+
+//
+void			btSoftBody::translate(const btVector3& trs)
+{
+	btTransform	t;
+	t.setIdentity();
+	t.setOrigin(trs);
+	transform(t);
+}
+
+//
+void			btSoftBody::rotate(	const btQuaternion& rot)
+{
+	btTransform	t;
+	t.setIdentity();
+	t.setRotation(rot);
+	transform(t);
+}
+
+//
+void			btSoftBody::scale(const btVector3& scl)
+{
+	const btScalar	margin=getCollisionShape()->getMargin();
+	for(int i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		n.m_x*=scl;
+		n.m_q*=scl;
+		m_ndbvt.update(n.m_leaf,btDbvtVolume::FromCR(n.m_x,margin));
+	}
+	updateNormals();
+	updateBounds();
+	updateConstants();
+}
+
+//
+void			btSoftBody::setPose(bool bvolume,bool bframe)
+{
+	m_pose.m_bvolume	=	bvolume;
+	m_pose.m_bframe		=	bframe;
+	int i,ni;
+
+	/* Weights		*/ 
+	const btScalar	omass=getTotalMass();
+	const btScalar	kmass=omass*m_nodes.size()*1000;
+	btScalar		tmass=omass;
+	m_pose.m_wgh.resize(m_nodes.size());
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		if(m_nodes[i].m_im<=0) tmass+=kmass;
+	}
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		m_pose.m_wgh[i]=	n.m_im>0					?
+			1/(m_nodes[i].m_im*tmass)	:
+		kmass/tmass;
+	}
+	/* Pos		*/ 
+	const btVector3	com=evaluateCom();
+	m_pose.m_pos.resize(m_nodes.size());
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		m_pose.m_pos[i]=m_nodes[i].m_x-com;
+	}
+	m_pose.m_volume	=	bvolume?getVolume():0;
+	m_pose.m_com	=	com;
+	m_pose.m_rot.setIdentity();
+	m_pose.m_scl.setIdentity();
+	/* Aqq		*/ 
+	m_pose.m_aqq[0]	=
+		m_pose.m_aqq[1]	=
+		m_pose.m_aqq[2]	=	btVector3(0,0,0);
+	for( i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		const btVector3&	q=m_pose.m_pos[i];
+		const btVector3		mq=m_pose.m_wgh[i]*q;
+		m_pose.m_aqq[0]+=mq.x()*q;
+		m_pose.m_aqq[1]+=mq.y()*q;
+		m_pose.m_aqq[2]+=mq.z()*q;
+	}
+	m_pose.m_aqq=m_pose.m_aqq.inverse();
+	updateConstants();
+}
+
+//
+btScalar		btSoftBody::getVolume() const
+{
+	btScalar	vol=0;
+	if(m_nodes.size()>0)
+	{
+		int i,ni;
+
+		const btVector3	org=m_nodes[0].m_x;
+		for(i=0,ni=m_faces.size();i<ni;++i)
+		{
+			const Face&	f=m_faces[i];
+			vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
+		}
+		vol/=(btScalar)6;
+	}
+	return(vol);
+}
+
+//
+int				btSoftBody::clusterCount() const
+{
+	return(m_clusters.size());
+}
+
+//
+btVector3		btSoftBody::clusterCom(const Cluster* cluster)
+{
+	btVector3		com(0,0,0);
+	for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
+	{
+		com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
+	}
+	return(com*cluster->m_imass);
+}
+
+//
+btVector3		btSoftBody::clusterCom(int cluster) const
+{
+	return(clusterCom(m_clusters[cluster]));
+}
+
+//
+btVector3		btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
+{
+	return(cluster->m_lv+cross(cluster->m_av,rpos));
+}
+
+//
+void			btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+	const btVector3	li=cluster->m_imass*impulse;
+	const btVector3	ai=cluster->m_invwi*cross(rpos,impulse);
+	cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
+	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	cluster->m_nvimpulses++;
+}
+
+//
+void			btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
+{
+	const btVector3	li=cluster->m_imass*impulse;
+	const btVector3	ai=cluster->m_invwi*cross(rpos,impulse);
+	cluster->m_dimpulses[0]+=li;
+	cluster->m_dimpulses[1]+=ai;
+	cluster->m_ndimpulses++;
+}
+
+//
+void			btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
+{
+	if(impulse.m_asVelocity)	clusterVImpulse(cluster,rpos,impulse.m_velocity);
+	if(impulse.m_asDrift)		clusterDImpulse(cluster,rpos,impulse.m_drift);
+}
+
+//
+void			btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	const btVector3	ai=cluster->m_invwi*impulse;
+	cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
+	cluster->m_nvimpulses++;
+}
+
+//
+void			btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	const btVector3	ai=cluster->m_invwi*impulse;
+	cluster->m_dimpulses[1]+=ai;
+	cluster->m_ndimpulses++;
+}
+
+//
+void			btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
+{
+	if(impulse.m_asVelocity)	clusterVAImpulse(cluster,impulse.m_velocity);
+	if(impulse.m_asDrift)		clusterDAImpulse(cluster,impulse.m_drift);
+}
+
+//
+void			btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
+{
+	cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
+	cluster->m_ndimpulses++;
+}
+
+//
+int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
+{
+	int i,j;
+
+	if(distance>1)
+	{
+		/* Build graph	*/ 
+		const int		n=m_nodes.size();
+		const unsigned	inf=(~(unsigned)0)>>1;
+		unsigned*		adj=new unsigned[n*n];
+#define IDX(_x_,_y_)	((_y_)*n+(_x_))
+		for(j=0;j<n;++j)
+		{
+			for(i=0;i<n;++i)
+			{
+				if(i!=j)	adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
+				else
+					adj[IDX(i,j)]=adj[IDX(j,i)]=0;
+			}
+		}
+		for( i=0;i<m_links.size();++i)
+		{
+			const int	ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
+			const int	ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
+			adj[IDX(ia,ib)]=1;
+			adj[IDX(ib,ia)]=1;
+		}
+		for(int k=0;k<n;++k)
+		{
+			for(j=0;j<n;++j)
+			{
+				for(i=j+1;i<n;++i)
+				{
+					const unsigned	sum=adj[IDX(i,k)]+adj[IDX(k,j)];
+					if(adj[IDX(i,j)]>sum)
+					{
+						adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
+					}
+				}
+			}
+		}
+		/* Build links	*/ 
+		int	nlinks=0;
+		for(j=0;j<n;++j)
+		{
+			for(i=j+1;i<n;++i)
+			{
+				if(adj[IDX(i,j)]==(unsigned)distance)
+				{
+					appendLink(i,j,mat);
+					m_links[m_links.size()-1].m_bbending=1;
+					++nlinks;
+				}
+			}
+		}
+		delete[] adj;		
+		return(nlinks);
+	}
+	return(0);
+}
+
+//
+void			btSoftBody::randomizeConstraints()
+{
+	unsigned long	seed=243703;
+#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
+	int i,ni;
+
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		btSwap(m_links[i],m_links[NEXTRAND%ni]);
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
+	}
+#undef NEXTRAND
+}
+
+//
+void			btSoftBody::releaseCluster(int index)
+{
+	Cluster*	c=m_clusters[index];
+	if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
+	c->~Cluster();
+	btAlignedFree(c);
+	m_clusters.remove(c);
+}
+
+//
+void			btSoftBody::releaseClusters()
+{
+	while(m_clusters.size()>0) releaseCluster(0);
+}
+
+//
+int				btSoftBody::generateClusters(int k,int maxiterations)
+{
+	int i;
+	releaseClusters();
+	m_clusters.resize(btMin(k,m_nodes.size()));
+	for(i=0;i<m_clusters.size();++i)
+	{
+		m_clusters[i]			=	new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+		m_clusters[i]->m_collide=	true;
+	}
+	k=m_clusters.size();
+	if(k>0)
+	{
+		/* Initialize		*/ 
+		btAlignedObjectArray<btVector3>	centers;
+		btVector3						cog(0,0,0);
+		int								i;
+		for(i=0;i<m_nodes.size();++i)
+		{
+			cog+=m_nodes[i].m_x;
+			m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
+		}
+		cog/=(btScalar)m_nodes.size();
+		centers.resize(k,cog);
+		/* Iterate			*/ 
+		const btScalar	slope=16;
+		bool			changed;
+		int				iterations=0;
+		do	{
+			const btScalar	w=2-btMin<btScalar>(1,iterations/slope);
+			changed=false;
+			iterations++;	
+			int i;
+
+			for(i=0;i<k;++i)
+			{
+				btVector3	c(0,0,0);
+				for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+				{
+					c+=m_clusters[i]->m_nodes[j]->m_x;
+				}
+				if(m_clusters[i]->m_nodes.size())
+				{
+					c			/=	(btScalar)m_clusters[i]->m_nodes.size();
+					c			=	centers[i]+(c-centers[i])*w;
+					changed		|=	((c-centers[i]).length2()>SIMD_EPSILON);
+					centers[i]	=	c;
+					m_clusters[i]->m_nodes.resize(0);
+				}			
+			}
+			for(i=0;i<m_nodes.size();++i)
+			{
+				const btVector3	nx=m_nodes[i].m_x;
+				int				kbest=0;
+				btScalar		kdist=ClusterMetric(centers[0],nx);
+				for(int j=1;j<k;++j)
+				{
+					const btScalar	d=ClusterMetric(centers[j],nx);
+					if(d<kdist)
+					{
+						kbest=j;
+						kdist=d;
+					}
+				}
+				m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
+			}		
+		} while(changed&&(iterations<maxiterations));
+		/* Merge		*/ 
+		btAlignedObjectArray<int>	cids;
+		cids.resize(m_nodes.size(),-1);
+		for(i=0;i<m_clusters.size();++i)
+		{
+			for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
+			{
+				cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
+			}
+		}
+		for(i=0;i<m_faces.size();++i)
+		{
+			const int idx[]={	int(m_faces[i].m_n[0]-&m_nodes[0]),
+				int(m_faces[i].m_n[1]-&m_nodes[0]),
+				int(m_faces[i].m_n[2]-&m_nodes[0])};
+			for(int j=0;j<3;++j)
+			{
+				const int cid=cids[idx[j]];
+				for(int q=1;q<3;++q)
+				{
+					const int kid=idx[(j+q)%3];
+					if(cids[kid]!=cid)
+					{
+						if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
+						{
+							m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
+						}
+					}
+				}
+			}
+		}
+		/* Master		*/ 
+		if(m_clusters.size()>1)
+		{
+			Cluster*	pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
+			pmaster->m_collide	=	false;
+			pmaster->m_nodes.reserve(m_nodes.size());
+			for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
+			m_clusters.push_back(pmaster);
+			btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
+		}
+		/* Terminate	*/ 
+		for(i=0;i<m_clusters.size();++i)
+		{
+			if(m_clusters[i]->m_nodes.size()==0)
+			{
+				releaseCluster(i--);
+			}
+		}
+
+		initializeClusters();
+		updateClusters();
+		return(m_clusters.size());
+	}
+	return(0);
+}
+
+//
+void			btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
+{
+	const Node*			nbase = &m_nodes[0];
+	int					ncount = m_nodes.size();
+	btSymMatrix<int>	edges(ncount,-2);
+	int					newnodes=0;
+	int i,j,k,ni;
+
+	/* Filter out		*/ 
+	for(i=0;i<m_links.size();++i)
+	{
+		Link&	l=m_links[i];
+		if(l.m_bbending)
+		{
+			if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
+			{
+				btSwap(m_links[i],m_links[m_links.size()-1]);
+				m_links.pop_back();--i;
+			}
+		}	
+	}
+	/* Fill edges		*/ 
+	for(i=0;i<m_links.size();++i)
+	{
+		Link&	l=m_links[i];
+		edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
+	}
+	for(i=0;i<m_faces.size();++i)
+	{	
+		Face&	f=m_faces[i];
+		edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
+		edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
+		edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
+	}
+	/* Intersect		*/ 
+	for(i=0;i<ncount;++i)
+	{
+		for(j=i+1;j<ncount;++j)
+		{
+			if(edges(i,j)==-1)
+			{
+				Node&			a=m_nodes[i];
+				Node&			b=m_nodes[j];
+				const btScalar	t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
+				if(t>0)
+				{
+					const btVector3	x=Lerp(a.m_x,b.m_x,t);
+					const btVector3	v=Lerp(a.m_v,b.m_v,t);
+					btScalar		m=0;
+					if(a.m_im>0)
+					{
+						if(b.m_im>0)
+						{
+							const btScalar	ma=1/a.m_im;
+							const btScalar	mb=1/b.m_im;
+							const btScalar	mc=Lerp(ma,mb,t);
+							const btScalar	f=(ma+mb)/(ma+mb+mc);
+							a.m_im=1/(ma*f);
+							b.m_im=1/(mb*f);
+							m=mc*f;
+						}
+						else
+						{ a.m_im/=0.5;m=1/a.m_im; }
+					}
+					else
+					{
+						if(b.m_im>0)
+						{ b.m_im/=0.5;m=1/b.m_im; }
+						else
+							m=0;
+					}
+					appendNode(x,m);
+					edges(i,j)=m_nodes.size()-1;
+					m_nodes[edges(i,j)].m_v=v;
+					++newnodes;
+				}
+			}
+		}
+	}
+	nbase=&m_nodes[0];
+	/* Refine links		*/ 
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		Link&		feat=m_links[i];
+		const int	idx[]={	int(feat.m_n[0]-nbase),
+			int(feat.m_n[1]-nbase)};
+		if((idx[0]<ncount)&&(idx[1]<ncount))
+		{
+			const int ni=edges(idx[0],idx[1]);
+			if(ni>0)
+			{
+				appendLink(i);
+				Link*		pft[]={	&m_links[i],
+					&m_links[m_links.size()-1]};			
+				pft[0]->m_n[0]=&m_nodes[idx[0]];
+				pft[0]->m_n[1]=&m_nodes[ni];
+				pft[1]->m_n[0]=&m_nodes[ni];
+				pft[1]->m_n[1]=&m_nodes[idx[1]];
+			}
+		}
+	}
+	/* Refine faces		*/ 
+	for(i=0;i<m_faces.size();++i)
+	{
+		const Face&	feat=m_faces[i];
+		const int	idx[]={	int(feat.m_n[0]-nbase),
+			int(feat.m_n[1]-nbase),
+			int(feat.m_n[2]-nbase)};
+		for(j=2,k=0;k<3;j=k++)
+		{
+			if((idx[j]<ncount)&&(idx[k]<ncount))
+			{
+				const int ni=edges(idx[j],idx[k]);
+				if(ni>0)
+				{
+					appendFace(i);
+					const int	l=(k+1)%3;
+					Face*		pft[]={	&m_faces[i],
+						&m_faces[m_faces.size()-1]};
+					pft[0]->m_n[0]=&m_nodes[idx[l]];
+					pft[0]->m_n[1]=&m_nodes[idx[j]];
+					pft[0]->m_n[2]=&m_nodes[ni];
+					pft[1]->m_n[0]=&m_nodes[ni];
+					pft[1]->m_n[1]=&m_nodes[idx[k]];
+					pft[1]->m_n[2]=&m_nodes[idx[l]];
+					appendLink(ni,idx[l],pft[0]->m_material);
+					--i;break;
+				}
+			}
+		}
+	}
+	/* Cut				*/ 
+	if(cut)
+	{	
+		btAlignedObjectArray<int>	cnodes;
+		const int					pcount=ncount;
+		int							i;
+		ncount=m_nodes.size();
+		cnodes.resize(ncount,0);
+		/* Nodes		*/ 
+		for(i=0;i<ncount;++i)
+		{
+			const btVector3	x=m_nodes[i].m_x;
+			if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
+			{
+				const btVector3	v=m_nodes[i].m_v;
+				btScalar		m=getMass(i);
+				if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
+				appendNode(x,m);
+				cnodes[i]=m_nodes.size()-1;
+				m_nodes[cnodes[i]].m_v=v;
+			}
+		}
+		nbase=&m_nodes[0];
+		/* Links		*/ 
+		for(i=0,ni=m_links.size();i<ni;++i)
+		{
+			const int		id[]={	int(m_links[i].m_n[0]-nbase),
+				int(m_links[i].m_n[1]-nbase)};
+			int				todetach=0;
+			if(cnodes[id[0]]&&cnodes[id[1]])
+			{
+				appendLink(i);
+				todetach=m_links.size()-1;
+			}
+			else
+			{
+				if((	(ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
+					(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
+					todetach=i;
+			}
+			if(todetach)
+			{
+				Link&	l=m_links[todetach];
+				for(int j=0;j<2;++j)
+				{
+					int cn=cnodes[int(l.m_n[j]-nbase)];
+					if(cn) l.m_n[j]=&m_nodes[cn];
+				}			
+			}
+		}
+		/* Faces		*/ 
+		for(i=0,ni=m_faces.size();i<ni;++i)
+		{
+			Node**			n=	m_faces[i].m_n;
+			if(	(ifn->Eval(n[0]->m_x)<accurary)&&
+				(ifn->Eval(n[1]->m_x)<accurary)&&
+				(ifn->Eval(n[2]->m_x)<accurary))
+			{
+				for(int j=0;j<3;++j)
+				{
+					int cn=cnodes[int(n[j]-nbase)];
+					if(cn) n[j]=&m_nodes[cn];
+				}
+			}
+		}
+		/* Clean orphans	*/ 
+		int							nnodes=m_nodes.size();
+		btAlignedObjectArray<int>	ranks;
+		btAlignedObjectArray<int>	todelete;
+		ranks.resize(nnodes,0);
+		for(i=0,ni=m_links.size();i<ni;++i)
+		{
+			for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
+		}
+		for(i=0,ni=m_faces.size();i<ni;++i)
+		{
+			for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
+		}
+		for(i=0;i<m_links.size();++i)
+		{
+			const int	id[]={	int(m_links[i].m_n[0]-nbase),
+				int(m_links[i].m_n[1]-nbase)};
+			const bool	sg[]={	ranks[id[0]]==1,
+				ranks[id[1]]==1};
+			if(sg[0]||sg[1])
+			{
+				--ranks[id[0]];
+				--ranks[id[1]];
+				btSwap(m_links[i],m_links[m_links.size()-1]);
+				m_links.pop_back();--i;
+			}
+		}
+#if 0	
+		for(i=nnodes-1;i>=0;--i)
+		{
+			if(!ranks[i]) todelete.push_back(i);
+		}	
+		if(todelete.size())
+		{		
+			btAlignedObjectArray<int>&	map=ranks;
+			for(int i=0;i<nnodes;++i) map[i]=i;
+			PointersToIndices(this);
+			for(int i=0,ni=todelete.size();i<ni;++i)
+			{
+				int		j=todelete[i];
+				int&	a=map[j];
+				int&	b=map[--nnodes];
+				m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
+				btSwap(m_nodes[a],m_nodes[b]);
+				j=a;a=b;b=j;			
+			}
+			IndicesToPointers(this,&map[0]);
+			m_nodes.resize(nnodes);
+		}
+#endif
+	}
+	m_bUpdateRtCst=true;
+}
+
+//
+bool			btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
+{
+	return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
+}
+
+//
+bool			btSoftBody::cutLink(int node0,int node1,btScalar position)
+{
+	bool			done=false;
+	int i,ni;
+	const btVector3	d=m_nodes[node0].m_x-m_nodes[node1].m_x;
+	const btVector3	x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
+	const btVector3	v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
+	const btScalar	m=1;
+	appendNode(x,m);
+	appendNode(x,m);
+	Node*			pa=&m_nodes[node0];
+	Node*			pb=&m_nodes[node1];
+	Node*			pn[2]={	&m_nodes[m_nodes.size()-2],
+		&m_nodes[m_nodes.size()-1]};
+	pn[0]->m_v=v;
+	pn[1]->m_v=v;
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
+		if(mtch!=-1)
+		{
+			appendLink(i);
+			Link*	pft[]={&m_links[i],&m_links[m_links.size()-1]};
+			pft[0]->m_n[1]=pn[mtch];
+			pft[1]->m_n[0]=pn[1-mtch];
+			done=true;
+		}
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		for(int k=2,l=0;l<3;k=l++)
+		{
+			const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
+			if(mtch!=-1)
+			{
+				appendFace(i);
+				Face*	pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
+				pft[0]->m_n[l]=pn[mtch];
+				pft[1]->m_n[k]=pn[1-mtch];
+				appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
+				appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
+			}
+		}
+	}
+	if(!done)
+	{
+		m_ndbvt.remove(pn[0]->m_leaf);
+		m_ndbvt.remove(pn[1]->m_leaf);
+		m_nodes.pop_back();
+		m_nodes.pop_back();
+	}
+	return(done);
+}
+
+//
+bool			btSoftBody::rayTest(const btVector3& rayFrom,
+									const btVector3& rayTo,
+									sRayCast& results)
+{
+	if(m_faces.size()&&m_fdbvt.empty()) 
+		initializeFaceTree();
+
+	results.body	=	this;
+	results.fraction = 1.f;
+	results.feature	=	eFeature::None;
+	results.index	=	-1;
+
+	return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
+}
+
+//
+void			btSoftBody::setSolver(eSolverPresets::_ preset)
+{
+	m_cfg.m_vsequence.clear();
+	m_cfg.m_psequence.clear();
+	m_cfg.m_dsequence.clear();
+	switch(preset)
+	{
+	case	eSolverPresets::Positions:
+		m_cfg.m_psequence.push_back(ePSolver::Anchors);
+		m_cfg.m_psequence.push_back(ePSolver::RContacts);
+		m_cfg.m_psequence.push_back(ePSolver::SContacts);
+		m_cfg.m_psequence.push_back(ePSolver::Linear);	
+		break;	
+	case	eSolverPresets::Velocities:
+		m_cfg.m_vsequence.push_back(eVSolver::Linear);
+
+		m_cfg.m_psequence.push_back(ePSolver::Anchors);
+		m_cfg.m_psequence.push_back(ePSolver::RContacts);
+		m_cfg.m_psequence.push_back(ePSolver::SContacts);
+
+		m_cfg.m_dsequence.push_back(ePSolver::Linear);
+		break;
+	}
+}
+
+//
+void			btSoftBody::predictMotion(btScalar dt)
+{
+	int i,ni;
+
+	/* Update				*/ 
+	if(m_bUpdateRtCst)
+	{
+		m_bUpdateRtCst=false;
+		updateConstants();
+		m_fdbvt.clear();
+		if(m_cfg.collisions&fCollision::VF_SS)
+		{
+			initializeFaceTree();			
+		}
+	}
+
+	/* Prepare				*/ 
+	m_sst.sdt		=	dt*m_cfg.timescale;
+	m_sst.isdt		=	1/m_sst.sdt;
+	m_sst.velmrg	=	m_sst.sdt*3;
+	m_sst.radmrg	=	getCollisionShape()->getMargin();
+	m_sst.updmrg	=	m_sst.radmrg*(btScalar)0.25;
+	/* Forces				*/ 
+	addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
+	applyForces();
+	/* Integrate			*/ 
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		n.m_q	=	n.m_x;
+		n.m_v	+=	n.m_f*n.m_im*m_sst.sdt;
+		n.m_x	+=	n.m_v*m_sst.sdt;
+		n.m_f	=	btVector3(0,0,0);
+	}
+	/* Clusters				*/ 
+	updateClusters();
+	/* Bounds				*/ 
+	updateBounds();	
+	/* Nodes				*/ 
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		Node&	n=m_nodes[i];
+		m_ndbvt.update(	n.m_leaf,
+			btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
+			n.m_v*m_sst.velmrg,
+			m_sst.updmrg);
+	}
+	/* Faces				*/ 
+	if(!m_fdbvt.empty())
+	{
+		for(int i=0;i<m_faces.size();++i)
+		{
+			Face&			f=m_faces[i];
+			const btVector3	v=(	f.m_n[0]->m_v+
+				f.m_n[1]->m_v+
+				f.m_n[2]->m_v)/3;
+			m_fdbvt.update(	f.m_leaf,
+				VolumeOf(f,m_sst.radmrg),
+				v*m_sst.velmrg,
+				m_sst.updmrg);
+		}
+	}
+	/* Pose					*/ 
+	updatePose();
+	/* Match				*/ 
+	if(m_pose.m_bframe&&(m_cfg.kMT>0))
+	{
+		const btMatrix3x3	posetrs=m_pose.m_rot;
+		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			Node&	n=m_nodes[i];
+			if(n.m_im>0)
+			{
+				const btVector3	x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
+				n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
+			}
+		}
+	}
+	/* Clear contacts		*/ 
+	m_rcontacts.resize(0);
+	m_scontacts.resize(0);
+	/* Optimize dbvt's		*/ 
+	m_ndbvt.optimizeIncremental(1);
+	m_fdbvt.optimizeIncremental(1);
+	m_cdbvt.optimizeIncremental(1);
+}
+
+//
+void			btSoftBody::solveConstraints()
+{
+	/* Apply clusters		*/ 
+	applyClusters(false);
+	/* Prepare links		*/ 
+
+	int i,ni;
+
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		Link&	l=m_links[i];
+		l.m_c3		=	l.m_n[1]->m_q-l.m_n[0]->m_q;
+		l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
+	}
+	/* Prepare anchors		*/ 
+	for(i=0,ni=m_anchors.size();i<ni;++i)
+	{
+		Anchor&			a=m_anchors[i];
+		const btVector3	ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
+		a.m_c0	=	ImpulseMatrix(	m_sst.sdt,
+			a.m_node->m_im,
+			a.m_body->getInvMass(),
+			a.m_body->getInvInertiaTensorWorld(),
+			ra);
+		a.m_c1	=	ra;
+		a.m_c2	=	m_sst.sdt*a.m_node->m_im;
+		a.m_body->activate();
+	}
+	/* Solve velocities		*/ 
+	if(m_cfg.viterations>0)
+	{
+		/* Solve			*/ 
+		for(int isolve=0;isolve<m_cfg.viterations;++isolve)
+		{
+			for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
+			{
+				getSolver(m_cfg.m_vsequence[iseq])(this,1);
+			}
+		}
+		/* Update			*/ 
+		for(i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			Node&	n=m_nodes[i];
+			n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
+		}
+	}
+	/* Solve positions		*/ 
+	if(m_cfg.piterations>0)
+	{
+		for(int isolve=0;isolve<m_cfg.piterations;++isolve)
+		{
+			const btScalar ti=isolve/(btScalar)m_cfg.piterations;
+			for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+			{
+				getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
+			}
+		}
+		const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
+		for(i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			Node&	n=m_nodes[i];
+			n.m_v	=	(n.m_x-n.m_q)*vc;
+			n.m_f	=	btVector3(0,0,0);		
+		}
+	}
+	/* Solve drift			*/ 
+	if(m_cfg.diterations>0)
+	{
+		const btScalar	vcf=m_cfg.kVCF*m_sst.isdt;
+		for(i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			Node&	n=m_nodes[i];
+			n.m_q	=	n.m_x;
+		}
+		for(int idrift=0;idrift<m_cfg.diterations;++idrift)
+		{
+			for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
+			{
+				getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
+			}
+		}
+		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			Node&	n=m_nodes[i];
+			n.m_v	+=	(n.m_x-n.m_q)*vcf;
+		}
+	}
+	/* Apply clusters		*/ 
+	dampClusters();
+	applyClusters(true);
+}
+
+//
+void			btSoftBody::staticSolve(int iterations)
+{
+	for(int isolve=0;isolve<iterations;++isolve)
+	{
+		for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
+		{
+			getSolver(m_cfg.m_psequence[iseq])(this,1,0);
+		}
+	}
+}
+
+//
+void			btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
+{
+	/// placeholder
+}
+
+//
+void			btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
+{
+	const int	nb=bodies.size();
+	int			iterations=0;
+	int i;
+
+	for(i=0;i<nb;++i)
+	{
+		iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
+	}
+	for(i=0;i<nb;++i)
+	{
+		bodies[i]->prepareClusters(iterations);
+	}
+	for(i=0;i<iterations;++i)
+	{
+		const btScalar sor=1;
+		for(int j=0;j<nb;++j)
+		{
+			bodies[j]->solveClusters(sor);
+		}
+	}
+	for(i=0;i<nb;++i)
+	{
+		bodies[i]->cleanupClusters();
+	}
+}
+
+//
+void			btSoftBody::integrateMotion()
+{
+	/* Update			*/ 
+	updateNormals();
+}
+
+//
+btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
+{
+	m_rayFrom = rayFrom;
+	m_rayNormalizedDirection = (rayTo-rayFrom);
+	m_rayTo = rayTo;
+	m_mint	=	mxt;
+	m_face	=	0;
+	m_tests	=	0;
+}
+
+//
+void				btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
+{
+	btSoftBody::Face&	f=*(btSoftBody::Face*)leaf->data;
+	const btScalar		t=rayFromToTriangle(	m_rayFrom,m_rayTo,m_rayNormalizedDirection,
+		f.m_n[0]->m_x,
+		f.m_n[1]->m_x,
+		f.m_n[2]->m_x,
+		m_mint);
+	if((t>0)&&(t<m_mint)) 
+	{ 
+		m_mint=t;m_face=&f; 
+	}
+	++m_tests;
+}
+
+//
+btScalar			btSoftBody::RayFromToCaster::rayFromToTriangle(	const btVector3& rayFrom,
+																   const btVector3& rayTo,
+																   const btVector3& rayNormalizedDirection,
+																   const btVector3& a,
+																   const btVector3& b,
+																   const btVector3& c,
+																   btScalar maxt)
+{
+	static const btScalar	ceps=-SIMD_EPSILON*10;
+	static const btScalar	teps=SIMD_EPSILON*10;
+
+	const btVector3			n=cross(b-a,c-a);
+	const btScalar			d=dot(a,n);
+	const btScalar			den=dot(rayNormalizedDirection,n);
+	if(!btFuzzyZero(den))
+	{
+		const btScalar		num=dot(rayFrom,n)-d;
+		const btScalar		t=-num/den;
+		if((t>teps)&&(t<maxt))
+		{
+			const btVector3	hit=rayFrom+rayNormalizedDirection*t;
+			if(	(dot(n,cross(a-hit,b-hit))>ceps)	&&			
+				(dot(n,cross(b-hit,c-hit))>ceps)	&&
+				(dot(n,cross(c-hit,a-hit))>ceps))
+			{
+				return(t);
+			}
+		}
+	}
+	return(-1);
+}
+
+//
+void				btSoftBody::pointersToIndices()
+{
+#define	PTR2IDX(_p_,_b_)	reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
+	btSoftBody::Node*	base=&m_nodes[0];
+	int i,ni;
+
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		if(m_nodes[i].m_leaf)
+		{
+			m_nodes[i].m_leaf->data=*(void**)&i;
+		}
+	}
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
+		m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
+		m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
+		m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
+		if(m_faces[i].m_leaf)
+		{
+			m_faces[i].m_leaf->data=*(void**)&i;
+		}
+	}
+	for(i=0,ni=m_anchors.size();i<ni;++i)
+	{
+		m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
+	}
+	for(i=0,ni=m_notes.size();i<ni;++i)
+	{
+		for(int j=0;j<m_notes[i].m_rank;++j)
+		{
+			m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
+		}
+	}
+#undef	PTR2IDX
+}
+
+//
+void				btSoftBody::indicesToPointers(const int* map)
+{
+#define	IDX2PTR(_p_,_b_)	map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):	\
+	(&(_b_)[(((char*)_p_)-(char*)0)])
+	btSoftBody::Node*	base=&m_nodes[0];
+	int i,ni;
+
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		if(m_nodes[i].m_leaf)
+		{
+			m_nodes[i].m_leaf->data=&m_nodes[i];
+		}
+	}
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
+		m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
+		m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
+		m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
+		if(m_faces[i].m_leaf)
+		{
+			m_faces[i].m_leaf->data=&m_faces[i];
+		}
+	}
+	for(i=0,ni=m_anchors.size();i<ni;++i)
+	{
+		m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
+	}
+	for(i=0,ni=m_notes.size();i<ni;++i)
+	{
+		for(int j=0;j<m_notes[i].m_rank;++j)
+		{
+			m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
+		}
+	}
+#undef	IDX2PTR
+}
+
+//
+int					btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
+										btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
+{
+	int	cnt=0;
+	if(bcountonly||m_fdbvt.empty())
+	{/* Full search	*/ 
+		btVector3 dir = rayTo-rayFrom;
+		dir.normalize();
+
+		for(int i=0,ni=m_faces.size();i<ni;++i)
+		{
+			const btSoftBody::Face&	f=m_faces[i];
+
+			const btScalar			t=RayFromToCaster::rayFromToTriangle(	rayFrom,rayTo,dir,
+				f.m_n[0]->m_x,
+				f.m_n[1]->m_x,
+				f.m_n[2]->m_x,
+				mint);
+			if(t>0)
+			{
+				++cnt;
+				if(!bcountonly)
+				{
+					feature=btSoftBody::eFeature::Face;
+					index=i;
+					mint=t;
+				}
+			}
+		}
+	}
+	else
+	{/* Use dbvt	*/ 
+		RayFromToCaster	collider(rayFrom,rayTo,mint);
+
+		btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
+		if(collider.m_face)
+		{
+			mint=collider.m_mint;
+			feature=btSoftBody::eFeature::Face;
+			index=(int)(collider.m_face-&m_faces[0]);
+			cnt=1;
+		}
+	}
+	return(cnt);
+}
+
+//
+void			btSoftBody::initializeFaceTree()
+{
+	m_fdbvt.clear();
+	for(int i=0;i<m_faces.size();++i)
+	{
+		Face&	f=m_faces[i];
+		f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
+	}
+}
+
+//
+btVector3		btSoftBody::evaluateCom() const
+{
+	btVector3	com(0,0,0);
+	if(m_pose.m_bframe)
+	{
+		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			com+=m_nodes[i].m_x*m_pose.m_wgh[i];
+		}
+	}
+	return(com);
+}
+
+//
+bool				btSoftBody::checkContact(	btRigidBody* prb,
+											 const btVector3& x,
+											 btScalar margin,
+											 btSoftBody::sCti& cti) const
+{
+	btVector3			nrm;
+	btCollisionShape*	shp=prb->getCollisionShape();
+	const btTransform&	wtr=prb->getInterpolationWorldTransform();
+	btScalar			dst=m_worldInfo->m_sparsesdf.Evaluate(	wtr.invXform(x),
+		shp,
+		nrm,
+		margin);
+	if(dst<0)
+	{
+		cti.m_body		=	prb;
+		cti.m_normal	=	wtr.getBasis()*nrm;
+		cti.m_offset	=	-dot(	cti.m_normal,
+			x-cti.m_normal*dst);
+		return(true);
+	}
+	return(false);
+}
+
+//
+void					btSoftBody::updateNormals()
+{
+	const btVector3	zv(0,0,0);
+	int i,ni;
+
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		m_nodes[i].m_n=zv;
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=m_faces[i];
+		const btVector3		n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
+			f.m_n[2]->m_x-f.m_n[0]->m_x);
+		f.m_normal=n.normalized();
+		f.m_n[0]->m_n+=n;
+		f.m_n[1]->m_n+=n;
+		f.m_n[2]->m_n+=n;
+	}
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		btScalar len = m_nodes[i].m_n.length();
+		if (len>SIMD_EPSILON)
+			m_nodes[i].m_n /= len;
+	}
+}
+
+//
+void					btSoftBody::updateBounds()
+{
+	if(m_ndbvt.m_root)
+	{
+		const btVector3&	mins=m_ndbvt.m_root->volume.Mins();
+		const btVector3&	maxs=m_ndbvt.m_root->volume.Maxs();
+		const btScalar		csm=getCollisionShape()->getMargin();
+		const btVector3		mrg=btVector3(	csm,
+			csm,
+			csm)*1; // ??? to investigate...
+		m_bounds[0]=mins-mrg;
+		m_bounds[1]=maxs+mrg;
+		if(0!=getBroadphaseHandle())
+		{					
+			m_worldInfo->m_broadphase->setAabb(	getBroadphaseHandle(),
+				m_bounds[0],
+				m_bounds[1],
+				m_worldInfo->m_dispatcher);
+		}
+	}
+	else
+	{
+		m_bounds[0]=
+			m_bounds[1]=btVector3(0,0,0);
+	}		
+}
+
+
+//
+void					btSoftBody::updatePose()
+{
+	if(m_pose.m_bframe)
+	{
+		btSoftBody::Pose&	pose=m_pose;
+		const btVector3		com=evaluateCom();
+		/* Com			*/ 
+		pose.m_com	=	com;
+		/* Rotation		*/ 
+		btMatrix3x3		Apq;
+		const btScalar	eps=SIMD_EPSILON;
+		Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
+		Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
+		for(int i=0,ni=m_nodes.size();i<ni;++i)
+		{
+			const btVector3		a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
+			const btVector3&	b=pose.m_pos[i];
+			Apq[0]+=a.x()*b;
+			Apq[1]+=a.y()*b;
+			Apq[2]+=a.z()*b;
+		}
+		btMatrix3x3		r,s;
+		PolarDecompose(Apq,r,s);
+		pose.m_rot=r;
+		pose.m_scl=pose.m_aqq*r.transpose()*Apq;
+		if(m_cfg.maxvolume>1)
+		{
+			const btScalar	idet=Clamp<btScalar>(	1/pose.m_scl.determinant(),
+				1,m_cfg.maxvolume);
+			pose.m_scl=Mul(pose.m_scl,idet);
+		}
+
+	}
+}
+
+//
+void				btSoftBody::updateConstants()
+{
+	int i,ni;
+
+	/* Links		*/ 
+	for(i=0,ni=m_links.size();i<ni;++i)
+	{
+		Link&		l=m_links[i];
+		Material&	m=*l.m_material;
+		l.m_rl	=	(l.m_n[0]->m_x-l.m_n[1]->m_x).length();
+		l.m_c0	=	(l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
+		l.m_c1	=	l.m_rl*l.m_rl;
+	}
+	/* Faces		*/ 
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		Face&		f=m_faces[i];
+		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
+	}
+	/* Area's		*/ 
+	btAlignedObjectArray<int>	counts;
+	counts.resize(m_nodes.size(),0);
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		m_nodes[i].m_area	=	0;
+	}
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=m_faces[i];
+		for(int j=0;j<3;++j)
+		{
+			const int index=(int)(f.m_n[j]-&m_nodes[0]);
+			counts[index]++;
+			f.m_n[j]->m_area+=btFabs(f.m_ra);
+		}
+	}
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		if(counts[i]>0)
+			m_nodes[i].m_area/=(btScalar)counts[i];
+		else
+			m_nodes[i].m_area=0;
+	}
+}
+
+//
+void					btSoftBody::initializeClusters()
+{
+	int i;
+
+	for( i=0;i<m_clusters.size();++i)
+	{
+		Cluster&	c=*m_clusters[i];
+		c.m_imass=0;
+		c.m_masses.resize(c.m_nodes.size());
+		for(int j=0;j<c.m_nodes.size();++j)
+		{
+			c.m_masses[j]	=	c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
+			c.m_imass		+=	c.m_masses[j];
+		}
+		c.m_imass		=	1/c.m_imass;
+		c.m_com			=	btSoftBody::clusterCom(&c);
+		c.m_lv			=	btVector3(0,0,0);
+		c.m_av			=	btVector3(0,0,0);
+		c.m_leaf		=	0;
+		/* Inertia	*/ 
+		btMatrix3x3&	ii=c.m_locii;
+		ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
+		{
+			int i,ni;
+
+			for(i=0,ni=c.m_nodes.size();i<ni;++i)
+			{
+				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
+				const btVector3	q=k*k;
+				const btScalar	m=c.m_masses[i];
+				ii[0][0]	+=	m*(q[1]+q[2]);
+				ii[1][1]	+=	m*(q[0]+q[2]);
+				ii[2][2]	+=	m*(q[0]+q[1]);
+				ii[0][1]	-=	m*k[0]*k[1];
+				ii[0][2]	-=	m*k[0]*k[2];
+				ii[1][2]	-=	m*k[1]*k[2];
+			}
+		}
+		ii[1][0]=ii[0][1];
+		ii[2][0]=ii[0][2];
+		ii[2][1]=ii[1][2];
+		ii=ii.inverse();
+		/* Frame	*/ 
+		c.m_framexform.setIdentity();
+		c.m_framexform.setOrigin(c.m_com);
+		c.m_framerefs.resize(c.m_nodes.size());
+		{
+			int i;
+			for(i=0;i<c.m_framerefs.size();++i)
+			{
+				c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
+			}
+		}
+	}
+}
+
+//
+void					btSoftBody::updateClusters()
+{
+	BT_PROFILE("UpdateClusters");
+	int i;
+
+	for(i=0;i<m_clusters.size();++i)
+	{
+		btSoftBody::Cluster&	c=*m_clusters[i];
+		const int				n=c.m_nodes.size();
+		const btScalar			invn=1/(btScalar)n;
+		if(n)
+		{
+			/* Frame				*/ 
+			const btScalar	eps=btScalar(0.0001);
+			btMatrix3x3		m,r,s;
+			m[0]=m[1]=m[2]=btVector3(0,0,0);
+			m[0][0]=eps*1;
+			m[1][1]=eps*2;
+			m[2][2]=eps*3;
+			c.m_com=clusterCom(&c);
+			for(int i=0;i<c.m_nodes.size();++i)
+			{
+				const btVector3		a=c.m_nodes[i]->m_x-c.m_com;
+				const btVector3&	b=c.m_framerefs[i];
+				m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
+			}
+			PolarDecompose(m,r,s);
+			c.m_framexform.setOrigin(c.m_com);
+			c.m_framexform.setBasis(r);		
+			/* Inertia			*/ 
+#if 1/* Constant	*/ 
+			c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
+#else
+#if 0/* Sphere	*/ 
+			const btScalar	rk=(2*c.m_extents.length2())/(5*c.m_imass);
+			const btVector3	inertia(rk,rk,rk);
+			const btVector3	iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
+				btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
+				btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
+
+			c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
+#else/* Actual	*/ 		
+			c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
+			for(int i=0;i<n;++i)
+			{
+				const btVector3	k=c.m_nodes[i]->m_x-c.m_com;
+				const btVector3		q=k*k;
+				const btScalar		m=1/c.m_nodes[i]->m_im;
+				c.m_invwi[0][0]	+=	m*(q[1]+q[2]);
+				c.m_invwi[1][1]	+=	m*(q[0]+q[2]);
+				c.m_invwi[2][2]	+=	m*(q[0]+q[1]);
+				c.m_invwi[0][1]	-=	m*k[0]*k[1];
+				c.m_invwi[0][2]	-=	m*k[0]*k[2];
+				c.m_invwi[1][2]	-=	m*k[1]*k[2];
+			}
+			c.m_invwi[1][0]=c.m_invwi[0][1];
+			c.m_invwi[2][0]=c.m_invwi[0][2];
+			c.m_invwi[2][1]=c.m_invwi[1][2];
+			c.m_invwi=c.m_invwi.inverse();
+#endif
+#endif
+			/* Velocities			*/ 
+			c.m_lv=btVector3(0,0,0);
+			c.m_av=btVector3(0,0,0);
+			{
+				int i;
+
+				for(i=0;i<n;++i)
+				{
+					const btVector3	v=c.m_nodes[i]->m_v*c.m_masses[i];
+					c.m_lv	+=	v;
+					c.m_av	+=	cross(c.m_nodes[i]->m_x-c.m_com,v);
+				}
+			}
+			c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
+			c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
+			c.m_vimpulses[0]	=
+				c.m_vimpulses[1]	= btVector3(0,0,0);
+			c.m_dimpulses[0]	=
+				c.m_dimpulses[1]	= btVector3(0,0,0);
+			c.m_nvimpulses		= 0;
+			c.m_ndimpulses		= 0;
+			/* Matching				*/ 
+			if(c.m_matching>0)
+			{
+				for(int j=0;j<c.m_nodes.size();++j)
+				{
+					Node&			n=*c.m_nodes[j];
+					const btVector3	x=c.m_framexform*c.m_framerefs[j];
+					n.m_x=Lerp(n.m_x,x,c.m_matching);
+				}
+			}
+			/* Dbvt					*/ 
+			if(c.m_collide)
+			{
+				btVector3	mi=c.m_nodes[0]->m_x;
+				btVector3	mx=mi;
+				for(int j=1;j<n;++j)
+				{
+					mi.setMin(c.m_nodes[j]->m_x);
+					mx.setMax(c.m_nodes[j]->m_x);
+				}			
+				const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(mi,mx);
+				if(c.m_leaf)
+					m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
+				else
+					c.m_leaf=m_cdbvt.insert(bounds,&c);
+			}
+		}
+	}
+}
+
+//
+void					btSoftBody::cleanupClusters()
+{
+	for(int i=0;i<m_joints.size();++i)
+	{
+		m_joints[i]->Terminate(m_sst.sdt);
+		if(m_joints[i]->m_delete)
+		{
+			btAlignedFree(m_joints[i]);
+			m_joints.remove(m_joints[i--]);
+		}	
+	}
+}
+
+//
+void					btSoftBody::prepareClusters(int iterations)
+{
+	for(int i=0;i<m_joints.size();++i)
+	{
+		m_joints[i]->Prepare(m_sst.sdt,iterations);
+	}
+}
+
+
+//
+void					btSoftBody::solveClusters(btScalar sor)
+{
+	for(int i=0,ni=m_joints.size();i<ni;++i)
+	{
+		m_joints[i]->Solve(m_sst.sdt,sor);
+	}
+}
+
+//
+void					btSoftBody::applyClusters(bool drift)
+{
+	BT_PROFILE("ApplyClusters");
+	const btScalar					f0=m_sst.sdt;
+	const btScalar					f1=f0/2;
+	btAlignedObjectArray<btVector3>	deltas;
+	btAlignedObjectArray<btScalar>	weights;
+	deltas.resize(m_nodes.size(),btVector3(0,0,0));
+	weights.resize(m_nodes.size(),0);
+	int i;
+
+	if(drift)
+	{
+		for(i=0;i<m_clusters.size();++i)
+		{
+			Cluster&	c=*m_clusters[i];
+			if(c.m_ndimpulses)
+			{
+				c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
+				c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
+			}
+		}
+	}
+	for(i=0;i<m_clusters.size();++i)
+	{
+		Cluster&	c=*m_clusters[i];	
+		if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
+		{
+			const btVector3		v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
+			const btVector3		w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
+			for(int j=0;j<c.m_nodes.size();++j)
+			{
+				const int			idx=int(c.m_nodes[j]-&m_nodes[0]);
+				const btVector3&	x=c.m_nodes[j]->m_x;
+				const btScalar		q=c.m_masses[j];
+				deltas[idx]		+=	(v+cross(w,x-c.m_com))*q;
+				weights[idx]	+=	q;
+			}
+		}
+	}
+	for(i=0;i<deltas.size();++i)
+	{
+		if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
+	}
+}
+
+//
+void					btSoftBody::dampClusters()
+{
+	int i;
+
+	for(i=0;i<m_clusters.size();++i)
+	{
+		Cluster&	c=*m_clusters[i];	
+		if(c.m_ndamping>0)
+		{
+			for(int j=0;j<c.m_nodes.size();++j)
+			{
+				Node&			n=*c.m_nodes[j];
+				if(n.m_im>0)
+				{
+					const btVector3	vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
+					n.m_v	+=	c.m_ndamping*(vx-n.m_v);
+				}
+			}
+		}
+	}
+}
+
+//
+void				btSoftBody::Joint::Prepare(btScalar dt,int)
+{
+	m_bodies[0].activate();
+	m_bodies[1].activate();
+}
+
+//
+void				btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
+{
+	static const btScalar	maxdrift=4;
+	Joint::Prepare(dt,iterations);
+	m_rpos[0]		=	m_bodies[0].xform()*m_refs[0];
+	m_rpos[1]		=	m_bodies[1].xform()*m_refs[1];
+	m_drift			=	Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
+	m_rpos[0]		-=	m_bodies[0].xform().getOrigin();
+	m_rpos[1]		-=	m_bodies[1].xform().getOrigin();
+	m_massmatrix	=	ImpulseMatrix(	m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
+		m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
+	if(m_split>0)
+	{
+		m_sdrift	=	m_massmatrix*(m_drift*m_split);
+		m_drift		*=	1-m_split;
+	}
+	m_drift	/=(btScalar)iterations;
+}
+
+//
+void				btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
+{
+	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
+	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
+	const btVector3		vr=va-vb;
+	btSoftBody::Impulse	impulse;
+	impulse.m_asVelocity	=	1;
+	impulse.m_velocity		=	m_massmatrix*(m_drift+vr*m_cfm)*sor;
+	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
+	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+}
+
+//
+void				btSoftBody::LJoint::Terminate(btScalar dt)
+{
+	if(m_split>0)
+	{
+		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
+		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+	}
+}
+
+//
+void				btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
+{
+	static const btScalar	maxdrift=SIMD_PI/16;
+	m_icontrol->Prepare(this);
+	Joint::Prepare(dt,iterations);
+	m_axis[0]	=	m_bodies[0].xform().getBasis()*m_refs[0];
+	m_axis[1]	=	m_bodies[1].xform().getBasis()*m_refs[1];
+	m_drift		=	NormalizeAny(cross(m_axis[1],m_axis[0]));
+	m_drift		*=	btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
+	m_drift		*=	m_erp/dt;
+	m_massmatrix=	AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
+	if(m_split>0)
+	{
+		m_sdrift	=	m_massmatrix*(m_drift*m_split);
+		m_drift		*=	1-m_split;
+	}
+	m_drift	/=(btScalar)iterations;
+}
+
+//
+void				btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
+{
+	const btVector3		va=m_bodies[0].angularVelocity();
+	const btVector3		vb=m_bodies[1].angularVelocity();
+	const btVector3		vr=va-vb;
+	const btScalar		sp=dot(vr,m_axis[0]);
+	const btVector3		vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
+	btSoftBody::Impulse	impulse;
+	impulse.m_asVelocity	=	1;
+	impulse.m_velocity		=	m_massmatrix*(m_drift+vc*m_cfm)*sor;
+	m_bodies[0].applyAImpulse(-impulse);
+	m_bodies[1].applyAImpulse( impulse);
+}
+
+//
+void				btSoftBody::AJoint::Terminate(btScalar dt)
+{
+	if(m_split>0)
+	{
+		m_bodies[0].applyDAImpulse(-m_sdrift);
+		m_bodies[1].applyDAImpulse( m_sdrift);
+	}
+}
+
+//
+void				btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
+{
+	Joint::Prepare(dt,iterations);
+	const bool	dodrift=(m_life==0);
+	m_delete=(++m_life)>m_maxlife;
+	if(dodrift)
+	{
+		m_drift=m_drift*m_erp/dt;
+		if(m_split>0)
+		{
+			m_sdrift	=	m_massmatrix*(m_drift*m_split);
+			m_drift		*=	1-m_split;
+		}
+		m_drift/=(btScalar)iterations;
+	}
+	else
+	{
+		m_drift=m_sdrift=btVector3(0,0,0);
+	}
+}
+
+//
+void				btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
+{
+	const btVector3		va=m_bodies[0].velocity(m_rpos[0]);
+	const btVector3		vb=m_bodies[1].velocity(m_rpos[1]);
+	const btVector3		vrel=va-vb;
+	const btScalar		rvac=dot(vrel,m_normal);
+	btSoftBody::Impulse	impulse;
+	impulse.m_asVelocity	=	1;
+	impulse.m_velocity		=	m_drift;
+	if(rvac<0)
+	{
+		const btVector3	iv=m_normal*rvac;
+		const btVector3	fv=vrel-iv;
+		impulse.m_velocity	+=	iv+fv*m_friction;
+	}
+	impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
+	m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
+	m_bodies[1].applyImpulse( impulse,m_rpos[1]);
+}
+
+//
+void				btSoftBody::CJoint::Terminate(btScalar dt)
+{
+	if(m_split>0)
+	{
+		m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
+		m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
+	}
+}
+
+//
+void				btSoftBody::applyForces()
+{
+	BT_PROFILE("SoftBody applyForces");
+	const btScalar					dt=m_sst.sdt;
+	const btScalar					kLF=m_cfg.kLF;
+	const btScalar					kDG=m_cfg.kDG;
+	const btScalar					kPR=m_cfg.kPR;
+	const btScalar					kVC=m_cfg.kVC;
+	const bool						as_lift=kLF>0;
+	const bool						as_drag=kDG>0;
+	const bool						as_pressure=kPR!=0;
+	const bool						as_volume=kVC>0;
+	const bool						as_aero=	as_lift		||
+		as_drag		;
+	const bool						as_vaero=	as_aero		&&
+		(m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
+	const bool						as_faero=	as_aero		&&
+		(m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
+	const bool						use_medium=	as_aero;
+	const bool						use_volume=	as_pressure	||
+		as_volume	;
+	btScalar						volume=0;
+	btScalar						ivolumetp=0;
+	btScalar						dvolumetv=0;
+	btSoftBody::sMedium	medium;
+	if(use_volume)
+	{
+		volume		=	getVolume();
+		ivolumetp	=	1/btFabs(volume)*kPR;
+		dvolumetv	=	(m_pose.m_volume-volume)*kVC;
+	}
+	/* Per vertex forces			*/ 
+	int i,ni;
+
+	for(i=0,ni=m_nodes.size();i<ni;++i)
+	{
+		btSoftBody::Node&	n=m_nodes[i];
+		if(n.m_im>0)
+		{
+			if(use_medium)
+			{
+				EvaluateMedium(m_worldInfo,n.m_x,medium);
+				/* Aerodynamics			*/ 
+				if(as_vaero)
+				{				
+					const btVector3	rel_v=n.m_v-medium.m_velocity;
+					const btScalar	rel_v2=rel_v.length2();
+					if(rel_v2>SIMD_EPSILON)
+					{
+						btVector3	nrm=n.m_n;
+						/* Setup normal		*/ 
+						switch(m_cfg.aeromodel)
+						{
+						case	btSoftBody::eAeroModel::V_Point:
+							nrm=NormalizeAny(rel_v);break;
+						case	btSoftBody::eAeroModel::V_TwoSided:
+							nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
+						}
+						const btScalar	dvn=dot(rel_v,nrm);
+						/* Compute forces	*/ 
+						if(dvn>0)
+						{
+							btVector3		force(0,0,0);
+							const btScalar	c0	=	n.m_area*dvn*rel_v2/2;
+							const btScalar	c1	=	c0*medium.m_density;
+							force	+=	nrm*(-c1*kLF);
+							force	+=	rel_v.normalized()*(-c1*kDG);
+							ApplyClampedForce(n,force,dt);
+						}
+					}
+				}
+			}
+			/* Pressure				*/ 
+			if(as_pressure)
+			{
+				n.m_f	+=	n.m_n*(n.m_area*ivolumetp);
+			}
+			/* Volume				*/ 
+			if(as_volume)
+			{
+				n.m_f	+=	n.m_n*(n.m_area*dvolumetv);
+			}
+		}
+	}
+	/* Per face forces				*/ 
+	for(i=0,ni=m_faces.size();i<ni;++i)
+	{
+		btSoftBody::Face&	f=m_faces[i];
+		if(as_faero)
+		{
+			const btVector3	v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
+			const btVector3	x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
+			EvaluateMedium(m_worldInfo,x,medium);
+			const btVector3	rel_v=v-medium.m_velocity;
+			const btScalar	rel_v2=rel_v.length2();
+			if(rel_v2>SIMD_EPSILON)
+			{
+				btVector3	nrm=f.m_normal;
+				/* Setup normal		*/ 
+				switch(m_cfg.aeromodel)
+				{
+				case	btSoftBody::eAeroModel::F_TwoSided:
+					nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
+				}
+				const btScalar	dvn=dot(rel_v,nrm);
+				/* Compute forces	*/ 
+				if(dvn>0)
+				{
+					btVector3		force(0,0,0);
+					const btScalar	c0	=	f.m_ra*dvn*rel_v2;
+					const btScalar	c1	=	c0*medium.m_density;
+					force	+=	nrm*(-c1*kLF);
+					force	+=	rel_v.normalized()*(-c1*kDG);
+					force	/=	3;
+					for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
+				}
+			}
+		}
+	}
+}
+
+//
+void				btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+	const btScalar	kAHR=psb->m_cfg.kAHR*kst;
+	const btScalar	dt=psb->m_sst.sdt;
+	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
+	{
+		const Anchor&		a=psb->m_anchors[i];
+		const btTransform&	t=a.m_body->getInterpolationWorldTransform();
+		Node&				n=*a.m_node;
+		const btVector3		wa=t*a.m_local;
+		const btVector3		va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
+		const btVector3		vb=n.m_x-n.m_q;
+		const btVector3		vr=(va-vb)+(wa-n.m_x)*kAHR;
+		const btVector3		impulse=a.m_c0*vr;
+		n.m_x+=impulse*a.m_c2;
+		a.m_body->applyImpulse(-impulse,a.m_c1);
+	}
+}
+
+//
+void				btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+	const btScalar	dt=psb->m_sst.sdt;
+	const btScalar	mrg=psb->getCollisionShape()->getMargin();
+	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
+	{
+		const RContact&		c=psb->m_rcontacts[i];
+		const sCti&			cti=c.m_cti;	
+		const btVector3		va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt;
+		const btVector3		vb=c.m_node->m_x-c.m_node->m_q;	
+		const btVector3		vr=vb-va;
+		const btScalar		dn=dot(vr,cti.m_normal);		
+		if(dn<=SIMD_EPSILON)
+		{
+			const btScalar		dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
+			const btVector3		fv=vr-cti.m_normal*dn;
+			const btVector3		impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
+			c.m_node->m_x-=impulse*c.m_c2;
+			c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
+		}
+	}
+}
+
+//
+void				btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
+{
+	for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
+	{
+		const SContact&		c=psb->m_scontacts[i];
+		const btVector3&	nr=c.m_normal;
+		Node&				n=*c.m_node;
+		Face&				f=*c.m_face;
+		const btVector3		p=BaryEval(	f.m_n[0]->m_x,
+			f.m_n[1]->m_x,
+			f.m_n[2]->m_x,
+			c.m_weights);
+		const btVector3		q=BaryEval(	f.m_n[0]->m_q,
+			f.m_n[1]->m_q,
+			f.m_n[2]->m_q,
+			c.m_weights);											
+		const btVector3		vr=(n.m_x-n.m_q)-(p-q);
+		btVector3			corr(0,0,0);
+		if(dot(vr,nr)<0)
+		{
+			const btScalar	j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
+			corr+=c.m_normal*j;
+		}
+		corr			-=	ProjectOnPlane(vr,nr)*c.m_friction;
+		n.m_x			+=	corr*c.m_cfm[0];
+		f.m_n[0]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.x());
+		f.m_n[1]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.y());
+		f.m_n[2]->m_x	-=	corr*(c.m_cfm[1]*c.m_weights.z());
+	}
+}
+
+//
+void				btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
+{
+	for(int i=0,ni=psb->m_links.size();i<ni;++i)
+	{			
+		Link&	l=psb->m_links[i];
+		if(l.m_c0>0)
+		{
+			Node&			a=*l.m_n[0];
+			Node&			b=*l.m_n[1];
+			const btVector3	del=b.m_x-a.m_x;
+			const btScalar	len=del.length2();
+			const btScalar	k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
+			const btScalar	t=k*a.m_im;
+			a.m_x-=del*(k*a.m_im);
+			b.m_x+=del*(k*b.m_im);
+		}
+	}
+}
+
+//
+void				btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
+{
+	for(int i=0,ni=psb->m_links.size();i<ni;++i)
+	{			
+		Link&			l=psb->m_links[i];
+		Node**			n=l.m_n;
+		const btScalar	j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
+		n[0]->m_v+=	l.m_c3*(j*n[0]->m_im);
+		n[1]->m_v-=	l.m_c3*(j*n[1]->m_im);
+	}
+}
+
+//
+btSoftBody::psolver_t	btSoftBody::getSolver(ePSolver::_ solver)
+{
+	switch(solver)
+	{
+	case	ePSolver::Anchors:		return(&btSoftBody::PSolve_Anchors);
+	case	ePSolver::Linear:		return(&btSoftBody::PSolve_Links);
+	case	ePSolver::RContacts:	return(&btSoftBody::PSolve_RContacts);
+	case	ePSolver::SContacts:	return(&btSoftBody::PSolve_SContacts);	
+	}
+	return(0);
+}
+
+//
+btSoftBody::vsolver_t	btSoftBody::getSolver(eVSolver::_ solver)
+{
+	switch(solver)
+	{
+	case	eVSolver::Linear:		return(&btSoftBody::VSolve_Links);
+	}
+	return(0);
+}
+
+//
+void			btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
+{
+	switch(m_cfg.collisions&fCollision::RVSmask)
+	{
+	case	fCollision::SDF_RS:
+		{
+			btSoftColliders::CollideSDF_RS	docollide;		
+			btRigidBody*		prb=btRigidBody::upcast(pco);
+			const btTransform	wtr=prb->getInterpolationWorldTransform();
+			const btTransform	ctr=prb->getWorldTransform();
+			const btScalar		timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
+			const btScalar		basemargin=getCollisionShape()->getMargin();
+			btVector3			mins;
+			btVector3			maxs;
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
+			pco->getCollisionShape()->getAabb(	pco->getInterpolationWorldTransform(),
+				mins,
+				maxs);
+			volume=btDbvtVolume::FromMM(mins,maxs);
+			volume.Expand(btVector3(basemargin,basemargin,basemargin));		
+			docollide.psb		=	this;
+			docollide.prb		=	prb;
+			docollide.dynmargin	=	basemargin+timemargin;
+			docollide.stamargin	=	basemargin;
+			btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
+		}
+		break;
+	case	fCollision::CL_RS:
+		{
+			btSoftColliders::CollideCL_RS	collider;
+			collider.Process(this,btRigidBody::upcast(pco));
+		}
+		break;
+	}
+}
+
+//
+void			btSoftBody::defaultCollisionHandler(btSoftBody* psb)
+{
+	const int cf=m_cfg.collisions&psb->m_cfg.collisions;
+	switch(cf&fCollision::SVSmask)
+	{
+	case	fCollision::CL_SS:
+		{
+			btSoftColliders::CollideCL_SS	docollide;
+			docollide.Process(this,psb);
+		}
+		break;
+	case	fCollision::VF_SS:
+		{
+			btSoftColliders::CollideVF_SS	docollide;
+			/* common					*/ 
+			docollide.mrg=	getCollisionShape()->getMargin()+
+				psb->getCollisionShape()->getMargin();
+			/* psb0 nodes vs psb1 faces	*/ 
+			docollide.psb[0]=this;
+			docollide.psb[1]=psb;
+			btDbvt::collideTT(	docollide.psb[0]->m_ndbvt.m_root,
+				docollide.psb[1]->m_fdbvt.m_root,
+				docollide);
+			/* psb1 nodes vs psb0 faces	*/ 
+			docollide.psb[0]=psb;
+			docollide.psb[1]=this;
+			btDbvt::collideTT(	docollide.psb[0]->m_ndbvt.m_root,
+				docollide.psb[1]->m_fdbvt.m_root,
+				docollide);
+		}
+		break;
+	}
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBody.h	(revision 1963)
@@ -0,0 +1,834 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#ifndef _BT_SOFT_BODY_H
+#define _BT_SOFT_BODY_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "btSparseSDF.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+
+class btBroadphaseInterface;
+class btDispatcher;
+
+/* btSoftBodyWorldInfo	*/ 
+struct	btSoftBodyWorldInfo
+{
+	btScalar				air_density;
+	btScalar				water_density;
+	btScalar				water_offset;
+	btVector3				water_normal;
+	btBroadphaseInterface*	m_broadphase;
+	btDispatcher*	m_dispatcher;
+	btVector3				m_gravity;
+	btSparseSdf<3>			m_sparsesdf;
+};	
+
+
+/// btSoftBody is work-in-progress
+class	btSoftBody : public btCollisionObject
+{
+public:
+	//
+	// Enumerations
+	//
+
+	///eAeroModel 
+	struct eAeroModel { enum _ {
+		V_Point,	///Vertex normals are oriented toward velocity
+		V_TwoSided,	///Vertex normals are fliped to match velocity	
+		V_OneSided,	///Vertex normals are taken as it is	
+		F_TwoSided,	///Face normals are fliped to match velocity
+		F_OneSided,	///Face normals are taken as it is
+		END
+	};};
+
+	///eVSolver : velocities solvers
+	struct	eVSolver { enum _ {
+		Linear,		///Linear solver
+		END
+	};};
+
+	///ePSolver : positions solvers
+	struct	ePSolver { enum _ {
+		Linear,		///Linear solver
+		Anchors,	///Anchor solver
+		RContacts,	///Rigid contacts solver
+		SContacts,	///Soft contacts solver
+		END
+	};};
+
+	///eSolverPresets
+	struct	eSolverPresets { enum _ {
+		Positions,
+		Velocities,
+		Default	=	Positions,
+		END
+	};};
+
+	///eFeature
+	struct	eFeature { enum _ {
+		None,
+		Node,
+		Link,
+		Face,
+		END
+	};};
+
+	typedef btAlignedObjectArray<eVSolver::_>	tVSolverArray;
+	typedef btAlignedObjectArray<ePSolver::_>	tPSolverArray;
+
+	//
+	// Flags
+	//
+
+	///fCollision
+	struct fCollision { enum _ {
+		RVSmask	=	0x000f,	///Rigid versus soft mask
+		SDF_RS	=	0x0001,	///SDF based rigid vs soft
+		CL_RS	=	0x0002, ///Cluster vs convex rigid vs soft
+
+		SVSmask	=	0x00f0,	///Rigid versus soft mask		
+		VF_SS	=	0x0010,	///Vertex vs face soft vs soft handling
+		CL_SS	=	0x0020, ///Cluster vs cluster soft vs soft handling
+		/* presets	*/ 
+		Default	=	SDF_RS,
+		END
+	};};
+
+	///fMaterial
+	struct fMaterial { enum _ {
+		DebugDraw	=	0x0001,	/// Enable debug draw
+		/* presets	*/ 
+		Default		=	DebugDraw,
+		END
+	};};
+
+	//
+	// API Types
+	//
+
+	/* sRayCast		*/ 
+	struct sRayCast
+	{
+		btSoftBody*	body;		/// soft body
+		eFeature::_	feature;	/// feature type
+		int			index;		/// feature index
+		btScalar	fraction;		/// time of impact fraction (rayorg+(rayto-rayfrom)*fraction)
+	};
+
+	/* ImplicitFn	*/ 
+	struct	ImplicitFn
+	{
+		virtual btScalar	Eval(const btVector3& x)=0;
+	};
+
+	//
+	// Internal types
+	//
+
+	typedef btAlignedObjectArray<btScalar>	tScalarArray;
+	typedef btAlignedObjectArray<btVector3>	tVector3Array;
+
+	/* sCti is Softbody contact info	*/ 
+	struct	sCti
+	{
+		btRigidBody*	m_body;		/* Rigid body			*/ 
+		btVector3		m_normal;	/* Outward normal		*/ 
+		btScalar		m_offset;	/* Offset from origin	*/ 
+	};	
+
+	/* sMedium		*/ 
+	struct	sMedium
+	{
+		btVector3		m_velocity;	/* Velocity				*/ 
+		btScalar		m_pressure;	/* Pressure				*/ 
+		btScalar		m_density;	/* Density				*/ 
+	};
+
+	/* Base type	*/ 
+	struct	Element
+	{
+		void*			m_tag;			// User data
+		Element() : m_tag(0) {}
+	};
+	/* Material		*/ 
+	struct	Material : Element
+	{
+		btScalar				m_kLST;			// Linear stiffness coefficient [0,1]
+		btScalar				m_kAST;			// Area/Angular stiffness coefficient [0,1]
+		btScalar				m_kVST;			// Volume stiffness coefficient [0,1]
+		int						m_flags;		// Flags
+	};
+
+	/* Feature		*/ 
+	struct	Feature : Element
+	{
+		Material*				m_material;		// Material
+	};
+	/* Node			*/ 
+	struct	Node : Feature
+	{
+		btVector3				m_x;			// Position
+		btVector3				m_q;			// Previous step position
+		btVector3				m_v;			// Velocity
+		btVector3				m_f;			// Force accumulator
+		btVector3				m_n;			// Normal
+		btScalar				m_im;			// 1/mass
+		btScalar				m_area;			// Area
+		btDbvtNode*				m_leaf;			// Leaf data
+		int						m_battach:1;	// Attached
+	};
+	/* Link			*/ 
+	struct	Link : Feature
+	{
+		Node*					m_n[2];			// Node pointers
+		btScalar				m_rl;			// Rest length		
+		int						m_bbending:1;	// Bending link
+		btScalar				m_c0;			// (ima+imb)*kLST
+		btScalar				m_c1;			// rl^2
+		btScalar				m_c2;			// |gradient|^2/c0
+		btVector3				m_c3;			// gradient
+	};
+	/* Face			*/ 
+	struct	Face : Feature
+	{
+		Node*					m_n[3];			// Node pointers
+		btVector3				m_normal;		// Normal
+		btScalar				m_ra;			// Rest area
+		btDbvtNode*				m_leaf;			// Leaf data
+	};
+	/* RContact		*/ 
+	struct	RContact
+	{
+		sCti		m_cti;			// Contact infos
+		Node*					m_node;			// Owner node
+		btMatrix3x3				m_c0;			// Impulse matrix
+		btVector3				m_c1;			// Relative anchor
+		btScalar				m_c2;			// ima*dt
+		btScalar				m_c3;			// Friction
+		btScalar				m_c4;			// Hardness
+	};
+	/* SContact		*/ 
+	struct	SContact
+	{
+		Node*					m_node;			// Node
+		Face*					m_face;			// Face
+		btVector3				m_weights;		// Weigths
+		btVector3				m_normal;		// Normal
+		btScalar				m_margin;		// Margin
+		btScalar				m_friction;		// Friction
+		btScalar				m_cfm[2];		// Constraint force mixing
+	};
+	/* Anchor		*/ 
+	struct	Anchor
+	{
+		Node*					m_node;			// Node pointer
+		btVector3				m_local;		// Anchor position in body space
+		btRigidBody*			m_body;			// Body
+		btMatrix3x3				m_c0;			// Impulse matrix
+		btVector3				m_c1;			// Relative anchor
+		btScalar				m_c2;			// ima*dt
+	};
+	/* Note			*/ 
+	struct	Note : Element
+	{
+		const char*				m_text;			// Text
+		btVector3				m_offset;		// Offset
+		int						m_rank;			// Rank
+		Node*					m_nodes[4];		// Nodes
+		btScalar				m_coords[4];	// Coordinates
+	};	
+	/* Pose			*/ 
+	struct	Pose
+	{
+		bool					m_bvolume;		// Is valid
+		bool					m_bframe;		// Is frame
+		btScalar				m_volume;		// Rest volume
+		tVector3Array			m_pos;			// Reference positions
+		tScalarArray			m_wgh;			// Weights
+		btVector3				m_com;			// COM
+		btMatrix3x3				m_rot;			// Rotation
+		btMatrix3x3				m_scl;			// Scale
+		btMatrix3x3				m_aqq;			// Base scaling
+	};
+	/* Cluster		*/ 
+	struct	Cluster
+	{		
+		btAlignedObjectArray<Node*>	m_nodes;		
+		tScalarArray				m_masses;
+		tVector3Array				m_framerefs;
+		btTransform					m_framexform;
+		btScalar					m_idmass;
+		btScalar					m_imass;
+		btMatrix3x3					m_locii;
+		btMatrix3x3					m_invwi;
+		btVector3					m_com;
+		btVector3					m_vimpulses[2];
+		btVector3					m_dimpulses[2];
+		int							m_nvimpulses;
+		int							m_ndimpulses;
+		btVector3					m_lv;
+		btVector3					m_av;
+		btDbvtNode*					m_leaf;
+		btScalar					m_ndamping;
+		btScalar					m_ldamping;
+		btScalar					m_adamping;
+		btScalar					m_matching;
+		bool						m_collide;
+		Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
+	};
+	/* Impulse		*/ 
+	struct	Impulse
+	{
+		btVector3					m_velocity;
+		btVector3					m_drift;
+		int							m_asVelocity:1;
+		int							m_asDrift:1;
+		Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0)	{}
+		Impulse						operator -() const
+		{
+			Impulse i=*this;
+			i.m_velocity=-i.m_velocity;
+			i.m_drift=-i.m_drift;
+			return(i);
+		}
+		Impulse						operator*(btScalar x) const
+		{
+			Impulse i=*this;
+			i.m_velocity*=x;
+			i.m_drift*=x;
+			return(i);
+		}
+	};
+	/* Body			*/ 
+	struct	Body
+	{
+		Cluster*		m_soft;
+		btRigidBody*				m_rigid;
+		Body() : m_soft(0),m_rigid(0)				{}
+		Body(Cluster* p) : m_soft(p),m_rigid(0)	{}
+		Body(btRigidBody* p) : m_soft(0),m_rigid(p)	{}
+		void						activate() const
+		{
+			if(m_rigid) m_rigid->activate();
+		}
+		const btMatrix3x3&			invWorldInertia() const
+		{
+			static const btMatrix3x3	iwi(0,0,0,0,0,0,0,0,0);
+			if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
+			if(m_soft)	return(m_soft->m_invwi);
+			return(iwi);
+		}
+		btScalar					invMass() const
+		{
+			if(m_rigid) return(m_rigid->getInvMass());
+			if(m_soft)	return(m_soft->m_imass);
+			return(0);
+		}
+		const btTransform&			xform() const
+		{
+			static const btTransform	identity=btTransform::getIdentity();		
+			if(m_rigid) return(m_rigid->getInterpolationWorldTransform());
+			if(m_soft)	return(m_soft->m_framexform);
+			return(identity);
+		}
+		btVector3					linearVelocity() const
+		{
+			if(m_rigid) return(m_rigid->getLinearVelocity());
+			if(m_soft)	return(m_soft->m_lv);
+			return(btVector3(0,0,0));
+		}
+		btVector3					angularVelocity(const btVector3& rpos) const
+		{			
+			if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos));
+			if(m_soft)	return(cross(m_soft->m_av,rpos));
+			return(btVector3(0,0,0));
+		}
+		btVector3					angularVelocity() const
+		{			
+			if(m_rigid) return(m_rigid->getAngularVelocity());
+			if(m_soft)	return(m_soft->m_av);
+			return(btVector3(0,0,0));
+		}
+		btVector3					velocity(const btVector3& rpos) const
+		{
+			return(linearVelocity()+angularVelocity(rpos));
+		}
+		void						applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
+		{
+			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
+			if(m_soft)	btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
+		}
+		void						applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
+		{
+			if(m_rigid)	m_rigid->applyImpulse(impulse,rpos);
+			if(m_soft)	btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
+		}		
+		void						applyImpulse(const Impulse& impulse,const btVector3& rpos) const
+		{
+			if(impulse.m_asVelocity)	applyVImpulse(impulse.m_velocity,rpos);
+			if(impulse.m_asDrift)		applyDImpulse(impulse.m_drift,rpos);
+		}
+		void						applyVAImpulse(const btVector3& impulse) const
+		{
+			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
+			if(m_soft)	btSoftBody::clusterVAImpulse(m_soft,impulse);
+		}
+		void						applyDAImpulse(const btVector3& impulse) const
+		{
+			if(m_rigid)	m_rigid->applyTorqueImpulse(impulse);
+			if(m_soft)	btSoftBody::clusterDAImpulse(m_soft,impulse);
+		}
+		void						applyAImpulse(const Impulse& impulse) const
+		{
+			if(impulse.m_asVelocity)	applyVAImpulse(impulse.m_velocity);
+			if(impulse.m_asDrift)		applyDAImpulse(impulse.m_drift);
+		}
+		void						applyDCImpulse(const btVector3& impulse) const
+		{
+			if(m_rigid)	m_rigid->applyCentralImpulse(impulse);
+			if(m_soft)	btSoftBody::clusterDCImpulse(m_soft,impulse);
+		}
+	};
+	/* Joint		*/ 
+	struct	Joint
+	{
+		struct eType { enum _ {
+			Linear,
+			Angular,
+			Contact,
+		};};
+		struct Specs
+		{
+			Specs() : erp(1),cfm(1),split(1) {}
+			btScalar	erp;
+			btScalar	cfm;
+			btScalar	split;
+		};
+		Body						m_bodies[2];
+		btVector3					m_refs[2];
+		btScalar					m_cfm;
+		btScalar					m_erp;
+		btScalar					m_split;
+		btVector3					m_drift;
+		btVector3					m_sdrift;
+		btMatrix3x3					m_massmatrix;
+		bool						m_delete;
+		virtual						~Joint() {}
+		Joint() : m_delete(false) {}
+		virtual void				Prepare(btScalar dt,int iterations);
+		virtual void				Solve(btScalar dt,btScalar sor)=0;
+		virtual void				Terminate(btScalar dt)=0;
+		virtual eType::_			Type() const=0;
+	};
+	/* LJoint		*/ 
+	struct	LJoint : Joint
+	{
+		struct Specs : Joint::Specs
+		{
+			btVector3	position;
+		};		
+		btVector3					m_rpos[2];
+		void						Prepare(btScalar dt,int iterations);
+		void						Solve(btScalar dt,btScalar sor);
+		void						Terminate(btScalar dt);
+		eType::_					Type() const { return(eType::Linear); }
+	};
+	/* AJoint		*/ 
+	struct	AJoint : Joint
+	{
+		struct IControl
+		{
+			virtual void			Prepare(AJoint*)				{}
+			virtual btScalar		Speed(AJoint*,btScalar current) { return(current); }
+			static IControl*		Default()						{ static IControl def;return(&def); }
+		};
+		struct Specs : Joint::Specs
+		{
+			Specs() : icontrol(IControl::Default()) {}
+			btVector3	axis;
+			IControl*	icontrol;
+		};		
+		btVector3					m_axis[2];
+		IControl*					m_icontrol;
+		void						Prepare(btScalar dt,int iterations);
+		void						Solve(btScalar dt,btScalar sor);
+		void						Terminate(btScalar dt);
+		eType::_					Type() const { return(eType::Angular); }
+	};
+	/* CJoint		*/ 
+	struct	CJoint : Joint
+	{		
+		int							m_life;
+		int							m_maxlife;
+		btVector3					m_rpos[2];
+		btVector3					m_normal;
+		btScalar					m_friction;
+		void						Prepare(btScalar dt,int iterations);
+		void						Solve(btScalar dt,btScalar sor);
+		void						Terminate(btScalar dt);
+		eType::_					Type() const { return(eType::Contact); }
+	};
+	/* Config		*/ 
+	struct	Config
+	{
+		eAeroModel::_			aeromodel;		// Aerodynamic model (default: V_Point)
+		btScalar				kVCF;			// Velocities correction factor (Baumgarte)
+		btScalar				kDP;			// Damping coefficient [0,1]
+		btScalar				kDG;			// Drag coefficient [0,+inf]
+		btScalar				kLF;			// Lift coefficient [0,+inf]
+		btScalar				kPR;			// Pressure coefficient [-inf,+inf]
+		btScalar				kVC;			// Volume conversation coefficient [0,+inf]
+		btScalar				kDF;			// Dynamic friction coefficient [0,1]
+		btScalar				kMT;			// Pose matching coefficient [0,1]		
+		btScalar				kCHR;			// Rigid contacts hardness [0,1]
+		btScalar				kKHR;			// Kinetic contacts hardness [0,1]
+		btScalar				kSHR;			// Soft contacts hardness [0,1]
+		btScalar				kAHR;			// Anchors hardness [0,1]
+		btScalar				kSRHR_CL;		// Soft vs rigid hardness [0,1] (cluster only)
+		btScalar				kSKHR_CL;		// Soft vs kinetic hardness [0,1] (cluster only)
+		btScalar				kSSHR_CL;		// Soft vs soft hardness [0,1] (cluster only)
+		btScalar				kSR_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar				kSK_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar				kSS_SPLT_CL;	// Soft vs rigid impulse split [0,1] (cluster only)
+		btScalar				maxvolume;		// Maximum volume ratio for pose
+		btScalar				timescale;		// Time scale
+		int						viterations;	// Velocities solver iterations
+		int						piterations;	// Positions solver iterations
+		int						diterations;	// Drift solver iterations
+		int						citerations;	// Cluster solver iterations
+		int						collisions;		// Collisions flags
+		tVSolverArray			m_vsequence;	// Velocity solvers sequence
+		tPSolverArray			m_psequence;	// Position solvers sequence
+		tPSolverArray			m_dsequence;	// Drift solvers sequence
+	};
+	/* SolverState	*/ 
+	struct	SolverState
+	{
+		btScalar				sdt;			// dt*timescale
+		btScalar				isdt;			// 1/sdt
+		btScalar				velmrg;			// velocity margin
+		btScalar				radmrg;			// radial margin
+		btScalar				updmrg;			// Update margin
+	};	
+	/// RayFromToCaster takes a ray from, ray to (instead of direction!)
+	struct	RayFromToCaster : btDbvt::ICollide
+	{
+		btVector3			m_rayFrom;
+		btVector3			m_rayTo;
+		btVector3			m_rayNormalizedDirection;
+		btScalar			m_mint;
+		Face*				m_face;
+		int					m_tests;
+		RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt);
+		void					Process(const btDbvtNode* leaf);
+
+		static inline btScalar	rayFromToTriangle(const btVector3& rayFrom,
+			const btVector3& rayTo,
+			const btVector3& rayNormalizedDirection,
+			const btVector3& a,
+			const btVector3& b,
+			const btVector3& c,
+			btScalar maxt=SIMD_INFINITY);
+	};
+
+	//
+	// Typedef's
+	//
+
+	typedef void								(*psolver_t)(btSoftBody*,btScalar,btScalar);
+	typedef void								(*vsolver_t)(btSoftBody*,btScalar);
+	typedef btAlignedObjectArray<Cluster*>		tClusterArray;
+	typedef btAlignedObjectArray<Note>			tNoteArray;
+	typedef btAlignedObjectArray<Node>			tNodeArray;
+	typedef btAlignedObjectArray<btDbvtNode*>	tLeafArray;
+	typedef btAlignedObjectArray<Link>			tLinkArray;
+	typedef btAlignedObjectArray<Face>			tFaceArray;
+	typedef btAlignedObjectArray<Anchor>		tAnchorArray;
+	typedef btAlignedObjectArray<RContact>		tRContactArray;
+	typedef btAlignedObjectArray<SContact>		tSContactArray;
+	typedef btAlignedObjectArray<Material*>		tMaterialArray;
+	typedef btAlignedObjectArray<Joint*>		tJointArray;
+	typedef btAlignedObjectArray<btSoftBody*>	tSoftBodyArray;	
+
+	//
+	// Fields
+	//
+
+	Config					m_cfg;			// Configuration
+	SolverState				m_sst;			// Solver state
+	Pose					m_pose;			// Pose
+	void*					m_tag;			// User data
+	btSoftBodyWorldInfo*	m_worldInfo;	// World info
+	tNoteArray				m_notes;		// Notes
+	tNodeArray				m_nodes;		// Nodes
+	tLinkArray				m_links;		// Links
+	tFaceArray				m_faces;		// Faces
+	tAnchorArray			m_anchors;		// Anchors
+	tRContactArray			m_rcontacts;	// Rigid contacts
+	tSContactArray			m_scontacts;	// Soft contacts
+	tJointArray				m_joints;		// Joints
+	tMaterialArray			m_materials;	// Materials
+	btScalar				m_timeacc;		// Time accumulator
+	btVector3				m_bounds[2];	// Spatial bounds	
+	bool					m_bUpdateRtCst;	// Update runtime constants
+	btDbvt					m_ndbvt;		// Nodes tree
+	btDbvt					m_fdbvt;		// Faces tree
+	btDbvt					m_cdbvt;		// Clusters tree
+	tClusterArray			m_clusters;		// Clusters
+
+	//
+	// Api
+	//
+
+	/* ctor																	*/ 
+	btSoftBody(	btSoftBodyWorldInfo* worldInfo,int node_count,
+		const btVector3* x,
+		const btScalar* m);
+	/* dtor																	*/ 
+	virtual ~btSoftBody();
+	/* Check for existing link												*/ 
+
+	btAlignedObjectArray<int>	m_userIndexMapping;
+
+	btSoftBodyWorldInfo*	getWorldInfo()
+	{
+		return m_worldInfo;
+	}
+
+	virtual void	setCollisionShape(btCollisionShape* collisionShape)
+	{
+		//don't do anything, due to the internal shape hack: todo: fix this
+	}
+
+	bool				checkLink(	int node0,
+		int node1) const;
+	bool				checkLink(	const Node* node0,
+		const Node* node1) const;
+	/* Check for existring face												*/ 
+	bool				checkFace(	int node0,
+		int node1,
+		int node2) const;
+	/* Append material														*/ 
+	Material*			appendMaterial();
+	/* Append note															*/ 
+	void				appendNote(	const char* text,
+		const btVector3& o,
+		const btVector4& c=btVector4(1,0,0,0),
+		Node* n0=0,
+		Node* n1=0,
+		Node* n2=0,
+		Node* n3=0);
+	void				appendNote(	const char* text,
+		const btVector3& o,
+		Node* feature);
+	void				appendNote(	const char* text,
+		const btVector3& o,
+		Link* feature);
+	void				appendNote(	const char* text,
+		const btVector3& o,
+		Face* feature);
+	/* Append node															*/ 
+	void				appendNode(	const btVector3& x,btScalar m);
+	/* Append link															*/ 
+	void				appendLink(int model=-1,Material* mat=0);
+	void				appendLink(	int node0,
+		int node1,
+		Material* mat=0,
+		bool bcheckexist=false);
+	void				appendLink(	Node* node0,
+		Node* node1,
+		Material* mat=0,
+		bool bcheckexist=false);
+	/* Append face															*/ 
+	void				appendFace(int model=-1,Material* mat=0);
+	void				appendFace(	int node0,
+		int node1,
+		int node2,
+		Material* mat=0);
+	/* Append anchor														*/ 
+	void				appendAnchor(	int node,
+		btRigidBody* body);
+	/* Append linear joint													*/ 
+	void				appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
+	void				appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
+	void				appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body);
+	/* Append linear joint													*/ 
+	void				appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1);
+	void				appendAngularJoint(const AJoint::Specs& specs,Body body=Body());
+	void				appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body);
+	/* Add force (or gravity) to the entire body							*/ 
+	void				addForce(		const btVector3& force);
+	/* Add force (or gravity) to a node of the body							*/ 
+	void				addForce(		const btVector3& force,
+		int node);
+	/* Add velocity to the entire body										*/ 
+	void				addVelocity(	const btVector3& velocity);
+
+	/* Set velocity for the entire body										*/ 
+	void				setVelocity(	const btVector3& velocity);
+
+	/* Add velocity to a node of the body									*/ 
+	void				addVelocity(	const btVector3& velocity,
+		int node);
+	/* Set mass																*/ 
+	void				setMass(		int node,
+		btScalar mass);
+	/* Get mass																*/ 
+	btScalar			getMass(		int node) const;
+	/* Get total mass														*/ 
+	btScalar			getTotalMass() const;
+	/* Set total mass (weighted by previous masses)							*/ 
+	void				setTotalMass(	btScalar mass,
+		bool fromfaces=false);
+	/* Set total density													*/ 
+	void				setTotalDensity(btScalar density);
+	/* Transform															*/ 
+	void				transform(		const btTransform& trs);
+	/* Translate															*/ 
+	void				translate(		const btVector3& trs);
+	/* Rotate															*/ 
+	void				rotate(	const btQuaternion& rot);
+	/* Scale																*/ 
+	void				scale(	const btVector3& scl);
+	/* Set current state as pose											*/ 
+	void				setPose(		bool bvolume,
+		bool bframe);
+	/* Return the volume													*/ 
+	btScalar			getVolume() const;
+	/* Cluster count														*/ 
+	int					clusterCount() const;
+	/* Cluster center of mass												*/ 
+	static btVector3	clusterCom(const Cluster* cluster);
+	btVector3			clusterCom(int cluster) const;
+	/* Cluster velocity at rpos												*/ 
+	static btVector3	clusterVelocity(const Cluster* cluster,const btVector3& rpos);
+	/* Cluster impulse														*/ 
+	static void			clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
+	static void			clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
+	static void			clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse);
+	static void			clusterVAImpulse(Cluster* cluster,const btVector3& impulse);
+	static void			clusterDAImpulse(Cluster* cluster,const btVector3& impulse);
+	static void			clusterAImpulse(Cluster* cluster,const Impulse& impulse);
+	static void			clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
+	/* Generate bending constraints based on distance in the adjency graph	*/ 
+	int					generateBendingConstraints(	int distance,
+		Material* mat=0);
+	/* Randomize constraints to reduce solver bias							*/ 
+	void				randomizeConstraints();
+	/* Release clusters														*/ 
+	void				releaseCluster(int index);
+	void				releaseClusters();
+	/* Generate clusters (K-mean)											*/ 
+	int					generateClusters(int k,int maxiterations=8192);
+	/* Refine																*/ 
+	void				refine(ImplicitFn* ifn,btScalar accurary,bool cut);
+	/* CutLink																*/ 
+	bool				cutLink(int node0,int node1,btScalar position);
+	bool				cutLink(const Node* node0,const Node* node1,btScalar position);
+
+	///Ray casting using rayFrom and rayTo in worldspace, (not direction!)
+	bool				rayTest(const btVector3& rayFrom,
+		const btVector3& rayTo,
+		sRayCast& results);
+	/* Solver presets														*/ 
+	void				setSolver(eSolverPresets::_ preset);
+	/* predictMotion														*/ 
+	void				predictMotion(btScalar dt);
+	/* solveConstraints														*/ 
+	void				solveConstraints();
+	/* staticSolve															*/ 
+	void				staticSolve(int iterations);
+	/* solveCommonConstraints												*/ 
+	static void			solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
+	/* solveClusters														*/ 
+	static void			solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
+	/* integrateMotion														*/ 
+	void				integrateMotion();
+	/* defaultCollisionHandlers												*/ 
+	void				defaultCollisionHandler(btCollisionObject* pco);
+	void				defaultCollisionHandler(btSoftBody* psb);
+
+	//
+	// Cast
+	//
+
+	static const btSoftBody*	upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_SOFT_BODY)
+			return (const btSoftBody*)colObj;
+		return 0;
+	}
+	static btSoftBody*			upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_SOFT_BODY)
+			return (btSoftBody*)colObj;
+		return 0;
+	}
+
+	//
+	// ::btCollisionObject
+	//
+
+	virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		aabbMin = m_bounds[0];
+		aabbMax = m_bounds[1];
+	}
+	//
+	// Private
+	//
+	void				pointersToIndices();
+	void				indicesToPointers(const int* map=0);
+
+	int					rayTest(const btVector3& rayFrom,const btVector3& rayTo,
+		btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
+	void				initializeFaceTree();
+	btVector3			evaluateCom() const;
+	bool				checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;
+	void				updateNormals();
+	void				updateBounds();
+	void				updatePose();
+	void				updateConstants();
+	void				initializeClusters();
+	void				updateClusters();
+	void				cleanupClusters();
+	void				prepareClusters(int iterations);
+	void				solveClusters(btScalar sor);
+	void				applyClusters(bool drift);
+	void				dampClusters();
+	void				applyForces();	
+	static void			PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti);
+	static void			PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti);
+	static void			PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti);
+	static void			PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti);
+	static void			VSolve_Links(btSoftBody* psb,btScalar kst);
+	static psolver_t	getSolver(ePSolver::_ solver);
+	static vsolver_t	getSolver(eVSolver::_ solver);
+
+};
+
+
+
+#endif //_BT_SOFT_BODY_H
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,369 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSoftBodyConcaveCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionShapes/btConcaveShape.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+
+
+
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
+
+btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
+: btCollisionAlgorithm(ci),
+m_isSwapped(isSwapped),
+m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
+{
+}
+
+
+
+btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
+{
+}
+
+
+
+btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
+m_dispatcher(dispatcher),
+m_dispatchInfoPtr(0)
+{
+	m_softBody = (btSoftBody*) (isSwapped? body1:body0);
+	m_triBody = isSwapped? body0:body1;
+
+	//
+	// create the manifold from the dispatcher 'manifold pool'
+	//
+	//	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
+
+	clearCache();
+}
+
+btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
+{
+	clearCache();
+	//	m_dispatcher->releaseManifold( m_manifoldPtr );
+
+}
+
+
+void	btSoftBodyTriangleCallback::clearCache()
+{
+	for (int i=0;i<m_shapeCache.size();i++)
+	{
+		btTriIndex* tmp = m_shapeCache.getAtIndex(i);
+		btAssert(tmp);
+		btAssert(tmp->m_childShape);
+		m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
+		delete tmp->m_childShape;
+	}
+	m_shapeCache.clear();
+};
+
+
+void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+	btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher1 = m_dispatcher;
+
+	///debug drawing of the overlapping triangles
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
+	{
+		btVector3 color(255,255,0);
+		btTransform& tr = ob->getWorldTransform();
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
+		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
+	}
+
+	btTriIndex	triIndex(partId,triangleIndex,0);
+	btHashKey<btTriIndex> triKey(triIndex.getUid());
+
+
+	btTriIndex* shapeIndex = m_shapeCache[triKey];
+	if (shapeIndex)
+	{
+		btCollisionShape* tm = shapeIndex->m_childShape;
+		btAssert(tm);
+
+		//copy over user pointers to temporary shape
+		tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
+
+		btCollisionShape* tmpShape = ob->getCollisionShape();
+		ob->internalSetTemporaryCollisionShape( tm );
+
+
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
+
+		colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+		ob->internalSetTemporaryCollisionShape( tmpShape);
+		return;
+	}
+
+	//aabb filter is already applied!	
+
+	//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+
+	//	if (m_softBody->getCollisionShape()->getShapeType()==
+	{
+		//		btVector3 other;
+		btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
+		normal.normalize();
+		normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
+		//		other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
+		//		other+=normal*22.f;
+		btVector3	pts[6] = {triangle[0]+normal,
+			triangle[1]+normal,
+			triangle[2]+normal,
+			triangle[0]-normal,
+			triangle[1]-normal,
+			triangle[2]-normal};
+
+		btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
+
+
+		//		btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
+
+		//btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		//	tm.setMargin(m_collisionMarginTriangle);
+
+		//copy over user pointers to temporary shape
+		tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
+
+		btCollisionShape* tmpShape = ob->getCollisionShape();
+		ob->internalSetTemporaryCollisionShape( tm );
+
+
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
+		///this should use the btDispatcher, so the actual registered algorithm is used
+		//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+
+		//m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
+		//	cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+		//		cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+
+
+		ob->internalSetTemporaryCollisionShape( tmpShape );
+		triIndex.m_childShape = tm;
+		m_shapeCache.insert(triKey,triIndex);
+
+	}
+
+
+
+}
+
+
+
+void	btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
+	m_resultOut = resultOut;
+
+
+	btVector3	aabbWorldSpaceMin,aabbWorldSpaceMax;
+	m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
+	btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
+	btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
+
+	btTransform softTransform;
+	softTransform.setIdentity();
+	softTransform.setOrigin(softBodyCenter);
+
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
+	btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
+}
+
+void btSoftBodyConcaveCollisionAlgorithm::clearCache()
+{
+	m_btSoftBodyTriangleCallback.clearCache();
+
+}
+
+void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+
+	btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+	if (triBody->getCollisionShape()->isConcave())
+	{
+
+
+		btCollisionObject*	triOb = triBody;
+		btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
+
+		//	if (convexBody->getCollisionShape()->isConvex())
+		{
+			btScalar collisionMarginTriangle = concaveShape->getMargin();
+
+			//			resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
+			m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
+
+			//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+			//m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
+
+			//			m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
+
+
+			concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
+
+			//	resultOut->refreshContactPoints();
+
+		}
+
+	}
+
+}
+
+
+btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
+	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
+
+
+	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+	//only perform CCD above a certain threshold, this prevents blocking on the long run
+	//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+	btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+	if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
+	{
+		return btScalar(1.);
+	}
+
+	//const btVector3& from = convexbody->m_worldTransform.getOrigin();
+	//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+	//todo: only do if the motion exceeds the 'radius'
+
+	btTransform triInv = triBody->getWorldTransform().inverse();
+	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
+
+	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	{
+		btTransform m_ccdSphereFromTrans;
+		btTransform m_ccdSphereToTrans;
+		btTransform	m_meshTransform;
+
+		btScalar	m_ccdSphereRadius;
+		btScalar	m_hitFraction;
+
+
+		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
+			:m_ccdSphereFromTrans(from),
+			m_ccdSphereToTrans(to),
+			m_ccdSphereRadius(ccdSphereRadius),
+			m_hitFraction(hitFraction)
+		{			
+		}
+
+
+		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)partId;
+			(void)triangleIndex;
+			//do a swept sphere for now
+			btTransform ident;
+			ident.setIdentity();
+			btConvexCast::CastResult castResult;
+			castResult.m_fraction = m_hitFraction;
+			btSphereShape	pointShape(m_ccdSphereRadius);
+			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
+			btVoronoiSimplexSolver	simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+			//local space?
+
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+				ident,ident,castResult))
+			{
+				if (m_hitFraction > castResult.m_fraction)
+					m_hitFraction = castResult.m_fraction;
+			}
+
+		}
+
+	};
+
+
+
+
+
+	if (triBody->getCollisionShape()->isConcave())
+	{
+		btVector3 rayAabbMin = convexFromLocal.getOrigin();
+		rayAabbMin.setMin(convexToLocal.getOrigin());
+		btVector3 rayAabbMax = convexFromLocal.getOrigin();
+		rayAabbMax.setMax(convexToLocal.getOrigin());
+		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+
+		btScalar curHitFraction = btScalar(1.); //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+
+		raycastCallback.m_hitFraction = convexbody->getHitFraction();
+
+		btCollisionObject* concavebody = triBody;
+
+		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
+
+		if (triangleMesh)
+		{
+			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+		}
+
+
+
+		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
+		{
+			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			return raycastCallback.m_hitFraction;
+		}
+	}
+
+	return btScalar(1.);
+
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
+#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btSoftBody;
+class btCollisionShape;
+
+#include "LinearMath/btHashMap.h"
+
+#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS
+
+struct btTriIndex
+{
+	int m_PartIdTriangleIndex;
+	class btCollisionShape*	m_childShape;
+
+	btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
+	{
+		m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+		m_childShape = shape;
+	}
+
+	int	getTriangleIndex() const
+	{
+		// Get only the lower bits where the triangle index is stored
+		return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
+	}
+	int	getPartId() const
+	{
+		// Get only the highest bits where the part index is stored
+		return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+	}
+	int	getUid() const
+	{
+		return m_PartIdTriangleIndex;
+	}
+};
+
+
+///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called.
+class btSoftBodyTriangleCallback : public btTriangleCallback
+{
+	btSoftBody* m_softBody;
+	btCollisionObject* m_triBody;
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax ;
+
+	btManifoldResult* m_resultOut;
+
+	btDispatcher*	m_dispatcher;
+	const btDispatcherInfo* m_dispatchInfoPtr;
+	btScalar m_collisionMarginTriangle;
+
+	btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
+
+public:
+	int	m_triangleCount;
+
+	//	btPersistentManifold*	m_manifoldPtr;
+
+	btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual ~btSoftBodyTriangleCallback();
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+	void clearCache();
+
+	SIMD_FORCE_INLINE const btVector3& getAabbMin() const
+	{
+		return m_aabbMin;
+	}
+	SIMD_FORCE_INLINE const btVector3& getAabbMax() const
+	{
+		return m_aabbMax;
+	}
+
+};
+
+
+
+
+/// btSoftBodyConcaveCollisionAlgorithm  supports collision between soft body shapes and (concave) trianges meshes.
+class btSoftBodyConcaveCollisionAlgorithm  : public btCollisionAlgorithm
+{
+
+	bool	m_isSwapped;
+
+	btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback;
+
+public:
+
+	btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
+
+	virtual ~btSoftBodyConcaveCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		//we don't add any manifolds
+	}
+
+	void	clearCache();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
+			return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
+			return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true);
+		}
+	};
+
+};
+
+#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.cpp	(revision 1963)
@@ -0,0 +1,857 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBodyHelpers.cpp by Nathanael Presson
+
+#include "btSoftBodyInternals.h"
+#include <stdio.h>
+#include <string.h>
+#include "btSoftBodyHelpers.h"
+#include "LinearMath/btConvexHull.h"
+
+//
+static void				drawVertex(	btIDebugDraw* idraw,
+								   const btVector3& x,btScalar s,const btVector3& c)
+{
+	idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
+	idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
+	idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
+}
+
+//
+static void				drawBox(	btIDebugDraw* idraw,
+								const btVector3& mins,
+								const btVector3& maxs,
+								const btVector3& color)
+{
+	const btVector3	c[]={	btVector3(mins.x(),mins.y(),mins.z()),
+		btVector3(maxs.x(),mins.y(),mins.z()),
+		btVector3(maxs.x(),maxs.y(),mins.z()),
+		btVector3(mins.x(),maxs.y(),mins.z()),
+		btVector3(mins.x(),mins.y(),maxs.z()),
+		btVector3(maxs.x(),mins.y(),maxs.z()),
+		btVector3(maxs.x(),maxs.y(),maxs.z()),
+		btVector3(mins.x(),maxs.y(),maxs.z())};
+	idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
+	idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
+	idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
+	idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
+	idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
+	idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
+}
+
+//
+static void				drawTree(	btIDebugDraw* idraw,
+								 const btDbvtNode* node,
+								 int depth,
+								 const btVector3& ncolor,
+								 const btVector3& lcolor,
+								 int mindepth,
+								 int maxdepth)
+{
+	if(node)
+	{
+		if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
+		{
+			drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
+			drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
+		}
+		if(depth>=mindepth)
+		{
+			const btScalar	scl=(btScalar)(node->isinternal()?1:1);
+			const btVector3	mi=node->volume.Center()-node->volume.Extents()*scl;
+			const btVector3	mx=node->volume.Center()+node->volume.Extents()*scl;
+			drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
+		}
+	}
+}
+
+//
+template <typename T>
+static inline T				sum(const btAlignedObjectArray<T>& items)
+{
+	T	v;
+	if(items.size())
+	{
+		v=items[0];
+		for(int i=1,ni=items.size();i<ni;++i)
+		{
+			v+=items[i];
+		}
+	}
+	return(v);
+}
+
+//
+template <typename T,typename Q>
+static inline void			add(btAlignedObjectArray<T>& items,const Q& value)
+{
+	for(int i=0,ni=items.size();i<ni;++i)
+	{
+		items[i]+=value;
+	}
+}
+
+//
+template <typename T,typename Q>
+static inline void			mul(btAlignedObjectArray<T>& items,const Q& value)
+{
+	for(int i=0,ni=items.size();i<ni;++i)
+	{
+		items[i]*=value;
+	}
+}
+
+//
+template <typename T>
+static inline T				average(const btAlignedObjectArray<T>& items)
+{
+	const btScalar	n=(btScalar)(items.size()>0?items.size():1);
+	return(sum(items)/n);
+}
+
+//
+static inline btScalar		tetravolume(const btVector3& x0,
+										const btVector3& x1,
+										const btVector3& x2,
+										const btVector3& x3)
+{
+	const btVector3	a=x1-x0;
+	const btVector3	b=x2-x0;
+	const btVector3	c=x3-x0;
+	return(dot(a,cross(b,c)));
+}
+
+//
+#if 0
+static btVector3		stresscolor(btScalar stress)
+{
+	static const btVector3	spectrum[]=	{	btVector3(1,0,1),
+		btVector3(0,0,1),
+		btVector3(0,1,1),
+		btVector3(0,1,0),
+		btVector3(1,1,0),
+		btVector3(1,0,0),
+		btVector3(1,0,0)};
+	static const int		ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
+	static const btScalar	one=1;
+	stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
+	const int				sel=(int)stress;
+	const btScalar			frc=stress-sel;
+	return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
+}
+#endif
+
+//
+void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
+										btIDebugDraw* idraw,
+										int drawflags)
+{
+	const btScalar		scl=(btScalar)0.1;
+	const btScalar		nscl=scl*5;
+	const btVector3		lcolor=btVector3(0,0,0);
+	const btVector3		ncolor=btVector3(1,1,1);
+	const btVector3		ccolor=btVector3(1,0,0);
+	int i,j,nj;
+
+	/* Nodes	*/ 
+	if(0!=(drawflags&fDrawFlags::Nodes))
+	{
+		for(i=0;i<psb->m_nodes.size();++i)
+		{
+			const btSoftBody::Node&	n=psb->m_nodes[i];
+			if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+			idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
+			idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
+			idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
+		}
+	}
+	/* Links	*/ 
+	if(0!=(drawflags&fDrawFlags::Links))
+	{
+		for(i=0;i<psb->m_links.size();++i)
+		{
+			const btSoftBody::Link&	l=psb->m_links[i];
+			if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+			idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
+		}
+	}
+	/* Normals	*/ 
+	if(0!=(drawflags&fDrawFlags::Normals))
+	{
+		for(i=0;i<psb->m_nodes.size();++i)
+		{
+			const btSoftBody::Node&	n=psb->m_nodes[i];
+			if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+			const btVector3			d=n.m_n*nscl;
+			idraw->drawLine(n.m_x,n.m_x+d,ncolor);
+			idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
+		}
+	}
+	/* Contacts	*/ 
+	if(0!=(drawflags&fDrawFlags::Contacts))
+	{
+		static const btVector3		axis[]={btVector3(1,0,0),
+			btVector3(0,1,0),
+			btVector3(0,0,1)};
+		for(i=0;i<psb->m_rcontacts.size();++i)
+		{		
+			const btSoftBody::RContact&	c=psb->m_rcontacts[i];
+			const btVector3				o=	c.m_node->m_x-c.m_cti.m_normal*
+				(dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
+			const btVector3				x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
+			const btVector3				y=cross(x,c.m_cti.m_normal).normalized();
+			idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
+			idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
+			idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
+		}
+	}
+	/* Anchors	*/ 
+	if(0!=(drawflags&fDrawFlags::Anchors))
+	{
+		for(i=0;i<psb->m_anchors.size();++i)
+		{
+			const btSoftBody::Anchor&	a=psb->m_anchors[i];
+			const btVector3				q=a.m_body->getWorldTransform()*a.m_local;
+			drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
+			drawVertex(idraw,q,0.25,btVector3(0,1,0));
+			idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
+		}
+		for(i=0;i<psb->m_nodes.size();++i)
+		{
+			const btSoftBody::Node&	n=psb->m_nodes[i];		
+			if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+			if(n.m_im<=0)
+			{
+				drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
+			}
+		}
+	}
+	/* Faces	*/ 
+	if(0!=(drawflags&fDrawFlags::Faces))
+	{
+		const btScalar	scl=(btScalar)0.8;
+		const btScalar	alp=(btScalar)1;
+		const btVector3	col(0,(btScalar)0.7,0);
+		for(i=0;i<psb->m_faces.size();++i)
+		{
+			const btSoftBody::Face&	f=psb->m_faces[i];
+			if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
+			const btVector3			x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
+			const btVector3			c=(x[0]+x[1]+x[2])/3;
+			idraw->drawTriangle((x[0]-c)*scl+c,
+				(x[1]-c)*scl+c,
+				(x[2]-c)*scl+c,
+				col,alp);
+		}	
+	}
+	/* Clusters	*/ 
+	if(0!=(drawflags&fDrawFlags::Clusters))
+	{
+		srand(1806);
+		for(i=0;i<psb->m_clusters.size();++i)
+		{
+			if(psb->m_clusters[i]->m_collide)
+			{
+				btVector3						color(	rand()/(btScalar)RAND_MAX,
+					rand()/(btScalar)RAND_MAX,
+					rand()/(btScalar)RAND_MAX);
+				color=color.normalized()*0.75;
+				btAlignedObjectArray<btVector3>	vertices;
+				vertices.resize(psb->m_clusters[i]->m_nodes.size());
+				for(j=0,nj=vertices.size();j<nj;++j)
+				{				
+					vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
+				}
+				HullDesc		hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
+				HullResult		hres;
+				HullLibrary		hlib;
+				hdsc.mMaxVertices=vertices.size();
+				hlib.CreateConvexHull(hdsc,hres);
+				const btVector3	center=average(hres.m_OutputVertices);
+				add(hres.m_OutputVertices,-center);
+				mul(hres.m_OutputVertices,(btScalar)1);
+				add(hres.m_OutputVertices,center);
+				for(j=0;j<(int)hres.mNumFaces;++j)
+				{
+					const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
+					idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
+						hres.m_OutputVertices[idx[1]],
+						hres.m_OutputVertices[idx[2]],
+						color,1);
+				}
+				hlib.ReleaseResult(hres);
+			}
+			/* Velocities	*/ 
+#if 0
+			for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
+			{
+				const btSoftBody::Cluster&	c=psb->m_clusters[i];
+				const btVector3				r=c.m_nodes[j]->m_x-c.m_com;
+				const btVector3				v=c.m_lv+cross(c.m_av,r);
+				idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
+			}
+#endif
+			/* Frame		*/ 
+			btSoftBody::Cluster& c=*psb->m_clusters[i];
+			idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
+			idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
+			idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
+		}
+	}
+	/* Notes	*/ 
+	if(0!=(drawflags&fDrawFlags::Notes))
+	{
+		for(i=0;i<psb->m_notes.size();++i)
+		{
+			const btSoftBody::Note&	n=psb->m_notes[i];
+			btVector3				p=n.m_offset;
+			for(int j=0;j<n.m_rank;++j)
+			{
+				p+=n.m_nodes[j]->m_x*n.m_coords[j];
+			}
+			idraw->draw3dText(p,n.m_text);
+		}
+	}
+	/* Node tree	*/ 
+	if(0!=(drawflags&fDrawFlags::NodeTree))		DrawNodeTree(psb,idraw);
+	/* Face tree	*/ 
+	if(0!=(drawflags&fDrawFlags::FaceTree))		DrawFaceTree(psb,idraw);
+	/* Cluster tree	*/ 
+	if(0!=(drawflags&fDrawFlags::ClusterTree))	DrawClusterTree(psb,idraw);
+	/* Joints		*/ 
+	if(0!=(drawflags&fDrawFlags::Joints))
+	{
+		for(i=0;i<psb->m_joints.size();++i)
+		{
+			const btSoftBody::Joint*	pj=psb->m_joints[i];
+			switch(pj->Type())
+			{
+			case	btSoftBody::Joint::eType::Linear:
+				{
+					const btSoftBody::LJoint*	pjl=(const btSoftBody::LJoint*)pj;
+					const btVector3	a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
+					const btVector3	a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
+					idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
+					idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
+					drawVertex(idraw,a0,0.25,btVector3(1,1,0));
+					drawVertex(idraw,a1,0.25,btVector3(0,1,1));
+				}
+				break;
+			case	btSoftBody::Joint::eType::Angular:
+				{
+					const btSoftBody::AJoint*	pja=(const btSoftBody::AJoint*)pj;
+					const btVector3	o0=pj->m_bodies[0].xform().getOrigin();
+					const btVector3	o1=pj->m_bodies[1].xform().getOrigin();
+					const btVector3	a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
+					const btVector3	a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
+					idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
+					idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
+					idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
+					idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
+				}
+			}		
+		}
+	}
+}
+
+//
+void			btSoftBodyHelpers::DrawInfos(		btSoftBody* psb,
+											 btIDebugDraw* idraw,
+											 bool masses,
+											 bool areas,
+											 bool /*stress*/)
+{
+	for(int i=0;i<psb->m_nodes.size();++i)
+	{
+		const btSoftBody::Node&	n=psb->m_nodes[i];
+		char					text[2048]={0};
+		char					buff[1024];
+		if(masses)
+		{
+			sprintf(buff," M(%.2f)",1/n.m_im);
+			strcat(text,buff);
+		}
+		if(areas)
+		{
+			sprintf(buff," A(%.2f)",n.m_area);
+			strcat(text,buff);
+		}
+		if(text[0]) idraw->draw3dText(n.m_x,text);
+	}
+}
+
+//
+void			btSoftBodyHelpers::DrawNodeTree(	btSoftBody* psb,
+												btIDebugDraw* idraw,
+												int mindepth,
+												int maxdepth)
+{
+	drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
+}
+
+//
+void			btSoftBodyHelpers::DrawFaceTree(	btSoftBody* psb,
+												btIDebugDraw* idraw,
+												int mindepth,
+												int maxdepth)
+{
+	drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
+}
+
+//
+void			btSoftBodyHelpers::DrawClusterTree(	btSoftBody* psb,
+												   btIDebugDraw* idraw,
+												   int mindepth,
+												   int maxdepth)
+{
+	drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
+}
+
+//
+void			btSoftBodyHelpers::DrawFrame(		btSoftBody* psb,
+											 btIDebugDraw* idraw)
+{
+	if(psb->m_pose.m_bframe)
+	{
+		static const btScalar	ascl=10;
+		static const btScalar	nscl=(btScalar)0.1;
+		const btVector3			com=psb->m_pose.m_com;
+		const btMatrix3x3		trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
+		const btVector3			Xaxis=(trs*btVector3(1,0,0)).normalized();
+		const btVector3			Yaxis=(trs*btVector3(0,1,0)).normalized();
+		const btVector3			Zaxis=(trs*btVector3(0,0,1)).normalized();
+		idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
+		idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
+		idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
+		for(int i=0;i<psb->m_pose.m_pos.size();++i)
+		{
+			const btVector3	x=com+trs*psb->m_pose.m_pos[i];
+			drawVertex(idraw,x,nscl,btVector3(1,0,1));
+		}
+	}
+}
+
+//
+btSoftBody*		btSoftBodyHelpers::CreateRope(	btSoftBodyWorldInfo& worldInfo, const btVector3& from,
+											  const btVector3& to,
+											  int res,
+											  int fixeds)
+{
+	/* Create nodes	*/ 
+	const int		r=res+2;
+	btVector3*		x=new btVector3[r];
+	btScalar*		m=new btScalar[r];
+	int i;
+
+	for(i=0;i<r;++i)
+	{
+		const btScalar	t=i/(btScalar)(r-1);
+		x[i]=lerp(from,to,t);
+		m[i]=1;
+	}
+	btSoftBody*		psb= new btSoftBody(&worldInfo,r,x,m);
+	if(fixeds&1) psb->setMass(0,0);
+	if(fixeds&2) psb->setMass(r-1,0);
+	delete[] x;
+	delete[] m;
+	/* Create links	*/ 
+	for(i=1;i<r;++i)
+	{
+		psb->appendLink(i-1,i);
+	}
+	/* Finished		*/ 
+	return(psb);
+}
+
+//
+btSoftBody*		btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
+											   const btVector3& corner10,
+											   const btVector3& corner01,
+											   const btVector3& corner11,
+											   int resx,
+											   int resy,
+											   int fixeds,
+											   bool gendiags)
+{
+#define IDX(_x_,_y_)	((_y_)*rx+(_x_))
+	/* Create nodes	*/ 
+	if((resx<2)||(resy<2)) return(0);
+	const int	rx=resx;
+	const int	ry=resy;
+	const int	tot=rx*ry;
+	btVector3*	x=new btVector3[tot];
+	btScalar*	m=new btScalar[tot];
+	int iy;
+
+	for(iy=0;iy<ry;++iy)
+	{
+		const btScalar	ty=iy/(btScalar)(ry-1);
+		const btVector3	py0=lerp(corner00,corner01,ty);
+		const btVector3	py1=lerp(corner10,corner11,ty);
+		for(int ix=0;ix<rx;++ix)
+		{
+			const btScalar	tx=ix/(btScalar)(rx-1);
+			x[IDX(ix,iy)]=lerp(py0,py1,tx);
+			m[IDX(ix,iy)]=1;
+		}
+	}
+	btSoftBody*		psb=new btSoftBody(&worldInfo,tot,x,m);
+	if(fixeds&1)	psb->setMass(IDX(0,0),0);
+	if(fixeds&2)	psb->setMass(IDX(rx-1,0),0);
+	if(fixeds&4)	psb->setMass(IDX(0,ry-1),0);
+	if(fixeds&8)	psb->setMass(IDX(rx-1,ry-1),0);
+	delete[] x;
+	delete[] m;
+	/* Create links	and faces */ 
+	for(iy=0;iy<ry;++iy)
+	{
+		for(int ix=0;ix<rx;++ix)
+		{
+			const int	idx=IDX(ix,iy);
+			const bool	mdx=(ix+1)<rx;
+			const bool	mdy=(iy+1)<ry;
+			if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
+			if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
+			if(mdx&&mdy)
+			{
+				if((ix+iy)&1)
+				{
+					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
+					psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
+					if(gendiags)
+					{
+						psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
+					}
+				}
+				else
+				{
+					psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
+					psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
+					if(gendiags)
+					{
+						psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
+					}
+				}
+			}
+		}
+	}
+	/* Finished		*/ 
+#undef IDX
+	return(psb);
+}
+
+//
+btSoftBody*		btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+												 const btVector3& corner00,
+												 const btVector3& corner10,
+												 const btVector3& corner01,
+												 const btVector3& corner11,
+												 int resx,
+												 int resy,
+												 int fixeds,
+												 bool gendiags,
+												 float* tex_coords)
+{
+
+	/*
+	*
+	*  corners:
+	*
+	*  [0][0]     corner00 ------- corner01   [resx][0]
+	*                |                |
+	*                |                |
+	*  [0][resy]  corner10 -------- corner11  [resx][resy]
+	*
+	*
+	*
+	*
+	*
+	*
+	*   "fixedgs" map:
+	*
+	*  corner00     -->   +1
+	*  corner01     -->   +2
+	*  corner10     -->   +4
+	*  corner11     -->   +8
+	*  upper middle -->  +16
+	*  left middle  -->  +32
+	*  right middle -->  +64
+	*  lower middle --> +128
+	*  center       --> +256
+	*
+	*
+	*   tex_coords size   (resx-1)*(resy-1)*12
+	*
+	*
+	*
+	*     SINGLE QUAD INTERNALS
+	*
+	*  1) btSoftBody's nodes and links,
+	*     diagonal link is optional ("gendiags")
+	*
+	*
+	*    node00 ------ node01
+	*      | .              
+	*      |   .            
+	*      |     .          
+	*      |       .        
+	*      |         .      
+	*    node10        node11
+	*
+	*
+	*
+	*   2) Faces:
+	*      two triangles,
+	*      UV Coordinates (hier example for single quad)
+	*      
+	*     (0,1)          (0,1)  (1,1)
+	*     1 |\            3 \-----| 2
+	*       | \              \    |
+	*       |  \              \   |
+	*       |   \              \  |
+	*       |    \              \ |
+	*     2 |-----\ 3            \| 1
+	*     (0,0)    (1,0)       (1,0)
+	*
+	*
+	*
+	*
+	*
+	*
+	*/
+
+#define IDX(_x_,_y_)	((_y_)*rx+(_x_))
+	/* Create nodes		*/ 
+	if((resx<2)||(resy<2)) return(0);
+	const int	rx=resx;
+	const int	ry=resy;
+	const int	tot=rx*ry;
+	btVector3*	x=new btVector3[tot];
+	btScalar*	m=new btScalar[tot];
+
+	for(int iy=0;iy<ry;++iy)
+	{
+		const btScalar	ty=iy/(btScalar)(ry-1);
+		const btVector3	py0=lerp(corner00,corner01,ty);
+		const btVector3	py1=lerp(corner10,corner11,ty);
+		for(int ix=0;ix<rx;++ix)
+		{
+			const btScalar	tx=ix/(btScalar)(rx-1);
+			x[IDX(ix,iy)]=lerp(py0,py1,tx);
+			m[IDX(ix,iy)]=1;
+		}
+	}
+	btSoftBody*	psb=new btSoftBody(&worldInfo,tot,x,m);
+	if(fixeds&1)		psb->setMass(IDX(0,0),0);
+	if(fixeds&2)		psb->setMass(IDX(rx-1,0),0);
+	if(fixeds&4)		psb->setMass(IDX(0,ry-1),0);
+	if(fixeds&8)		psb->setMass(IDX(rx-1,ry-1),0);
+	if(fixeds&16)		psb->setMass(IDX((rx-1)/2,0),0);
+	if(fixeds&32)		psb->setMass(IDX(0,(ry-1)/2),0);
+	if(fixeds&64)		psb->setMass(IDX(rx-1,(ry-1)/2),0);
+	if(fixeds&128)		psb->setMass(IDX((rx-1)/2,ry-1),0);
+	if(fixeds&256)		psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
+	delete[] x;
+	delete[] m;
+
+
+	int z = 0;
+	/* Create links	and faces	*/ 
+	for(int iy=0;iy<ry;++iy)
+	{
+		for(int ix=0;ix<rx;++ix)
+		{
+			const bool	mdx=(ix+1)<rx;
+			const bool	mdy=(iy+1)<ry;
+
+			int node00=IDX(ix,iy);
+			int node01=IDX(ix+1,iy);
+			int node10=IDX(ix,iy+1);
+			int node11=IDX(ix+1,iy+1);
+
+			if(mdx) psb->appendLink(node00,node01);
+			if(mdy) psb->appendLink(node00,node10);
+			if(mdx&&mdy)
+			{
+				psb->appendFace(node00,node10,node11);
+				if (tex_coords) {
+					tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
+					tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
+					tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
+					tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
+					tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
+					tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
+				}
+				psb->appendFace(node11,node01,node00);
+				if (tex_coords) {
+					tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
+					tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
+					tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
+					tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
+					tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
+					tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
+				}
+				if (gendiags) psb->appendLink(node00,node11);
+				z += 12;
+			}
+		}
+	}
+	/* Finished	*/ 
+#undef IDX
+	return(psb);
+}
+
+float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
+{
+
+	/*
+	*
+	*
+	*    node00 --- node01
+	*      |          |
+	*    node10 --- node11
+	*
+	*
+	*   ID map:
+	*
+	*   node00 s --> 0
+	*   node00 t --> 1
+	*
+	*   node01 s --> 3
+	*   node01 t --> 1
+	*
+	*   node10 s --> 0
+	*   node10 t --> 2
+	*
+	*   node11 s --> 3
+	*   node11 t --> 2
+	*
+	*
+	*/
+
+	float tc=0.0f;
+	if (id == 0) {
+		tc = (1.0f/((resx-1))*ix);
+	}
+	else if (id==1) {
+		tc = (1.0f/((resy-1))*(resy-1-iy));
+	}
+	else if (id==2) {
+		tc = (1.0f/((resy-1))*(resy-1-iy-1));
+	}
+	else if (id==3) {
+		tc = (1.0f/((resx-1))*(ix+1));
+	}
+	return tc;
+}
+//
+btSoftBody*		btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
+												   const btVector3& radius,
+												   int res)
+{
+	struct	Hammersley
+	{
+		static void	Generate(btVector3* x,int n)
+		{
+			for(int i=0;i<n;i++)
+			{
+				btScalar	p=0.5,t=0;
+				for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
+				btScalar	w=2*t-1;
+				btScalar	a=(SIMD_PI+2*i*SIMD_PI)/n;
+				btScalar	s=btSqrt(1-w*w);
+				*x++=btVector3(s*btCos(a),s*btSin(a),w);
+			}
+		}
+	};
+	btAlignedObjectArray<btVector3>	vtx;
+	vtx.resize(3+res);
+	Hammersley::Generate(&vtx[0],vtx.size());
+	for(int i=0;i<vtx.size();++i)
+	{
+		vtx[i]=vtx[i]*radius+center;
+	}
+	return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
+}
+
+
+
+//
+btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*	vertices,
+													 const int* triangles,
+													 int ntriangles)
+{
+	int		maxidx=0;
+	int i,j,ni;
+
+	for(i=0,ni=ntriangles*3;i<ni;++i)
+	{
+		maxidx=btMax(triangles[i],maxidx);
+	}
+	++maxidx;
+	btAlignedObjectArray<bool>		chks;
+	btAlignedObjectArray<btVector3>	vtx;
+	chks.resize(maxidx*maxidx,false);
+	vtx.resize(maxidx);
+	for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
+	{
+		vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
+	}
+	btSoftBody*		psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
+	for( i=0,ni=ntriangles*3;i<ni;i+=3)
+	{
+		const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
+#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
+		for(int j=2,k=0;k<3;j=k++)
+		{
+			if(!chks[IDX(idx[j],idx[k])])
+			{
+				chks[IDX(idx[j],idx[k])]=true;
+				chks[IDX(idx[k],idx[k])]=true;
+				psb->appendLink(idx[j],idx[k]);
+			}
+		}
+#undef IDX
+		psb->appendFace(idx[0],idx[1],idx[2]);
+	}
+	psb->randomizeConstraints();
+	return(psb);
+}
+
+//
+btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo,	const btVector3* vertices,
+														int nvertices)
+{
+	HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
+	HullResult		hres;
+	HullLibrary		hlib;/*??*/ 
+	hdsc.mMaxVertices=nvertices;
+	hlib.CreateConvexHull(hdsc,hres);
+	btSoftBody*		psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
+		&hres.m_OutputVertices[0],0);
+	for(int i=0;i<(int)hres.mNumFaces;++i)
+	{
+		const int idx[]={	hres.m_Indices[i*3+0],
+			hres.m_Indices[i*3+1],
+			hres.m_Indices[i*3+2]};
+		if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1]);
+		if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2]);
+		if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0]);
+		psb->appendFace(idx[0],idx[1],idx[2]);
+	}
+	hlib.ReleaseResult(hres);
+	psb->randomizeConstraints();
+	return(psb);
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyHelpers.h	(revision 1963)
@@ -0,0 +1,119 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_BODY_HELPERS_H
+#define SOFT_BODY_HELPERS_H
+
+#include "btSoftBody.h"
+
+//
+// Helpers
+//
+
+/* fDrawFlags															*/ 
+struct	fDrawFlags { enum _ {
+	Nodes		=	0x0001,
+	Links		=	0x0002,
+	Faces		=	0x0004,
+	Tetras		=	0x0008,
+	Normals		=	0x0010,
+	Contacts	=	0x0020,
+	Anchors		=	0x0040,
+	Notes		=	0x0080,
+	Clusters	=	0x0100,
+	NodeTree	=	0x0200,
+	FaceTree	=	0x0400,
+	ClusterTree	=	0x0800,
+	Joints		=	0x1000,
+	/* presets	*/ 
+	Std			=	Links+Faces+Tetras+Anchors+Notes+Joints,
+	StdTetra	=	Std-Faces+Tetras,
+};};
+
+struct	btSoftBodyHelpers
+{
+	/* Draw body															*/ 
+	static void				Draw(		btSoftBody* psb,
+		btIDebugDraw* idraw,
+		int drawflags=fDrawFlags::Std);
+	/* Draw body infos														*/ 
+	static	void			DrawInfos(	btSoftBody* psb,
+		btIDebugDraw* idraw,
+		bool masses,
+		bool areas,
+		bool stress);
+	/* Draw node tree														*/ 
+	static void				DrawNodeTree(	btSoftBody* psb,
+		btIDebugDraw* idraw,
+		int mindepth=0,
+		int maxdepth=-1);
+	/* Draw face tree														*/ 
+	static void				DrawFaceTree(	btSoftBody* psb,
+		btIDebugDraw* idraw,
+		int mindepth=0,
+		int maxdepth=-1);
+	/* Draw cluster tree													*/ 
+	static void				DrawClusterTree(btSoftBody* psb,
+		btIDebugDraw* idraw,
+		int mindepth=0,
+		int maxdepth=-1);
+	/* Draw rigid frame														*/ 
+	static	void			DrawFrame(		btSoftBody* psb,
+		btIDebugDraw* idraw);
+	/* Create a rope														*/ 
+	static	btSoftBody*		CreateRope( btSoftBodyWorldInfo& worldInfo,
+		const btVector3& from,
+		const btVector3& to,
+		int res,
+		int fixeds);
+	/* Create a patch														*/ 
+	static	btSoftBody*		CreatePatch(btSoftBodyWorldInfo& worldInfo,
+		const btVector3& corner00,
+		const btVector3& corner10,
+		const btVector3& corner01,
+		const btVector3& corner11,
+		int resx,
+		int resy,
+		int fixeds,
+		bool gendiags);
+	/* Create a patch with UV Texture Coordinates	*/ 
+	static	btSoftBody*		CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
+		const btVector3& corner00,
+		const btVector3& corner10,
+		const btVector3& corner01,
+		const btVector3& corner11,
+		int resx,
+		int resy,
+		int fixeds,
+		bool gendiags,
+		float* tex_coords=0);
+	static	float	CalculateUV(int resx,int resy,int ix,int iy,int id);
+	/* Create an ellipsoid													*/ 
+	static	btSoftBody*		CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,
+		const btVector3& center,
+		const btVector3& radius,
+		int res);	
+	/* Create from trimesh													*/ 
+	static	btSoftBody*		CreateFromTriMesh(	btSoftBodyWorldInfo& worldInfo,
+		const btScalar*	vertices,
+		const int* triangles,
+		int ntriangles);
+	/* Create from convex-hull												*/ 
+	static	btSoftBody*		CreateFromConvexHull(	btSoftBodyWorldInfo& worldInfo,
+		const btVector3* vertices,
+		int nvertices);
+};
+
+#endif //SOFT_BODY_HELPERS_H
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyInternals.h	(revision 1963)
@@ -0,0 +1,893 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+#ifndef _BT_SOFT_BODY_INTERNALS_H
+#define _BT_SOFT_BODY_INTERNALS_H
+
+#include "btSoftBody.h"
+
+#include "LinearMath/btQuickprof.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+//
+// btSymMatrix
+//
+template <typename T>
+struct btSymMatrix
+{
+	btSymMatrix() : dim(0)					{}
+	btSymMatrix(int n,const T& init=T())	{ resize(n,init); }
+	void					resize(int n,const T& init=T())			{ dim=n;store.resize((n*(n+1))/2,init); }
+	int						index(int c,int r) const				{ if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
+	T&						operator()(int c,int r)					{ return(store[index(c,r)]); }
+	const T&				operator()(int c,int r) const			{ return(store[index(c,r)]); }
+	btAlignedObjectArray<T>	store;
+	int						dim;
+};	
+
+//
+// btSoftBodyCollisionShape
+//
+class btSoftBodyCollisionShape : public btConcaveShape
+{
+public:
+	btSoftBody*						m_body;
+
+	btSoftBodyCollisionShape(btSoftBody* backptr)
+	{
+		m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
+		m_body=backptr;
+	}
+
+	virtual ~btSoftBodyCollisionShape()
+	{
+
+	}
+
+	void	processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
+	{
+		//not yet
+		btAssert(0);
+	}
+
+	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+	{
+		/* t should be identity, but better be safe than...fast? */ 
+		const btVector3	mins=m_body->m_bounds[0];
+		const btVector3	maxs=m_body->m_bounds[1];
+		const btVector3	crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
+			t*btVector3(maxs.x(),mins.y(),mins.z()),
+			t*btVector3(maxs.x(),maxs.y(),mins.z()),
+			t*btVector3(mins.x(),maxs.y(),mins.z()),
+			t*btVector3(mins.x(),mins.y(),maxs.z()),
+			t*btVector3(maxs.x(),mins.y(),maxs.z()),
+			t*btVector3(maxs.x(),maxs.y(),maxs.z()),
+			t*btVector3(mins.x(),maxs.y(),maxs.z())};
+		aabbMin=aabbMax=crns[0];
+		for(int i=1;i<8;++i)
+		{
+			aabbMin.setMin(crns[i]);
+			aabbMax.setMax(crns[i]);
+		}
+	}
+
+
+	virtual void	setLocalScaling(const btVector3& /*scaling*/)
+	{		
+		///na
+	}
+	virtual const btVector3& getLocalScaling() const
+	{
+		static const btVector3 dummy(1,1,1);
+		return dummy;
+	}
+	virtual void	calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
+	{
+		///not yet
+		btAssert(0);
+	}
+	virtual const char*	getName()const
+	{
+		return "SoftBody";
+	}
+
+};
+
+//
+// btSoftClusterCollisionShape
+//
+class btSoftClusterCollisionShape : public btConvexInternalShape
+{
+public:
+	const btSoftBody::Cluster*	m_cluster;
+
+	btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
+
+
+	virtual btVector3	localGetSupportingVertex(const btVector3& vec) const
+	{
+		btSoftBody::Node* const *						n=&m_cluster->m_nodes[0];
+		btScalar										d=dot(vec,n[0]->m_x);
+		int												j=0;
+		for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
+		{
+			const btScalar	k=dot(vec,n[i]->m_x);
+			if(k>d) { d=k;j=i; }
+		}
+		return(n[j]->m_x);
+	}
+	virtual btVector3	localGetSupportingVertexWithoutMargin(const btVector3& vec)const
+	{
+		return(localGetSupportingVertex(vec));
+	}
+	//notice that the vectors should be unit length
+	virtual void	batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+	{}
+
+
+	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
+	{}
+
+	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+	{}
+
+	virtual int	getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
+
+	//debugging
+	virtual const char*	getName()const {return "SOFTCLUSTER";}
+
+	virtual void	setMargin(btScalar margin)
+	{
+		btConvexInternalShape::setMargin(margin);
+	}
+	virtual btScalar	getMargin() const
+	{
+		return getMargin();
+	}
+};
+
+//
+// Inline's
+//
+
+//
+template <typename T>
+static inline void			ZeroInitialize(T& value)
+{
+	static const T	zerodummy;
+	value=zerodummy;
+}
+//
+template <typename T>
+static inline bool			CompLess(const T& a,const T& b)
+{ return(a<b); }
+//
+template <typename T>
+static inline bool			CompGreater(const T& a,const T& b)
+{ return(a>b); }
+//
+template <typename T>
+static inline T				Lerp(const T& a,const T& b,btScalar t)
+{ return(a+(b-a)*t); }
+//
+template <typename T>
+static inline T				InvLerp(const T& a,const T& b,btScalar t)
+{ return((b+a*t-b*t)/(a*b)); }
+//
+static inline btMatrix3x3	Lerp(	const btMatrix3x3& a,
+								 const btMatrix3x3& b,
+								 btScalar t)
+{
+	btMatrix3x3	r;
+	r[0]=Lerp(a[0],b[0],t);
+	r[1]=Lerp(a[1],b[1],t);
+	r[2]=Lerp(a[2],b[2],t);
+	return(r);
+}
+//
+static inline btVector3		Clamp(const btVector3& v,btScalar maxlength)
+{
+	const btScalar sql=v.length2();
+	if(sql>(maxlength*maxlength))
+		return((v*maxlength)/btSqrt(sql));
+	else
+		return(v);
+}
+//
+template <typename T>
+static inline T				Clamp(const T& x,const T& l,const T& h)
+{ return(x<l?l:x>h?h:x); }
+//
+template <typename T>
+static inline T				Sq(const T& x)
+{ return(x*x); }
+//
+template <typename T>
+static inline T				Cube(const T& x)
+{ return(x*x*x); }
+//
+template <typename T>
+static inline T				Sign(const T& x)
+{ return((T)(x<0?-1:+1)); }
+//
+template <typename T>
+static inline bool			SameSign(const T& x,const T& y)
+{ return((x*y)>0); }
+//
+static inline btScalar		ClusterMetric(const btVector3& x,const btVector3& y)
+{
+	const btVector3	d=x-y;
+	return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
+}
+//
+static inline btMatrix3x3	ScaleAlongAxis(const btVector3& a,btScalar s)
+{
+	const btScalar	xx=a.x()*a.x();
+	const btScalar	yy=a.y()*a.y();
+	const btScalar	zz=a.z()*a.z();
+	const btScalar	xy=a.x()*a.y();
+	const btScalar	yz=a.y()*a.z();
+	const btScalar	zx=a.z()*a.x();
+	btMatrix3x3		m;
+	m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
+	m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
+	m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
+	return(m);
+}
+//
+static inline btMatrix3x3	Cross(const btVector3& v)
+{
+	btMatrix3x3	m;
+	m[0]=btVector3(0,-v.z(),+v.y());
+	m[1]=btVector3(+v.z(),0,-v.x());
+	m[2]=btVector3(-v.y(),+v.x(),0);
+	return(m);
+}
+//
+static inline btMatrix3x3	Diagonal(btScalar x)
+{
+	btMatrix3x3	m;
+	m[0]=btVector3(x,0,0);
+	m[1]=btVector3(0,x,0);
+	m[2]=btVector3(0,0,x);
+	return(m);
+}
+//
+static inline btMatrix3x3	Add(const btMatrix3x3& a,
+								const btMatrix3x3& b)
+{
+	btMatrix3x3	r;
+	for(int i=0;i<3;++i) r[i]=a[i]+b[i];
+	return(r);
+}
+//
+static inline btMatrix3x3	Sub(const btMatrix3x3& a,
+								const btMatrix3x3& b)
+{
+	btMatrix3x3	r;
+	for(int i=0;i<3;++i) r[i]=a[i]-b[i];
+	return(r);
+}
+//
+static inline btMatrix3x3	Mul(const btMatrix3x3& a,
+								btScalar b)
+{
+	btMatrix3x3	r;
+	for(int i=0;i<3;++i) r[i]=a[i]*b;
+	return(r);
+}
+//
+static inline void			Orthogonalize(btMatrix3x3& m)
+{
+	m[2]=cross(m[0],m[1]).normalized();
+	m[1]=cross(m[2],m[0]).normalized();
+	m[0]=cross(m[1],m[2]).normalized();
+}
+//
+static inline btMatrix3x3	MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
+{
+	const btMatrix3x3	cr=Cross(r);
+	return(Sub(Diagonal(im),cr*iwi*cr));
+}
+
+//
+static inline btMatrix3x3	ImpulseMatrix(	btScalar dt,
+										  btScalar ima,
+										  btScalar imb,
+										  const btMatrix3x3& iwi,
+										  const btVector3& r)
+{
+	return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
+}
+
+//
+static inline btMatrix3x3	ImpulseMatrix(	btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
+										  btScalar imb,const btMatrix3x3& iib,const btVector3& rb)	
+{
+	return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
+}
+
+//
+static inline btMatrix3x3	AngularImpulseMatrix(	const btMatrix3x3& iia,
+												 const btMatrix3x3& iib)
+{
+	return(Add(iia,iib).inverse());
+}
+
+//
+static inline btVector3		ProjectOnAxis(	const btVector3& v,
+										  const btVector3& a)
+{
+	return(a*dot(v,a));
+}
+//
+static inline btVector3		ProjectOnPlane(	const btVector3& v,
+										   const btVector3& a)
+{
+	return(v-ProjectOnAxis(v,a));
+}
+
+//
+static inline void			ProjectOrigin(	const btVector3& a,
+										  const btVector3& b,
+										  btVector3& prj,
+										  btScalar& sqd)
+{
+	const btVector3	d=b-a;
+	const btScalar	m2=d.length2();
+	if(m2>SIMD_EPSILON)
+	{	
+		const btScalar	t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
+		const btVector3	p=a+d*t;
+		const btScalar	l2=p.length2();
+		if(l2<sqd)
+		{
+			prj=p;
+			sqd=l2;
+		}
+	}
+}
+//
+static inline void			ProjectOrigin(	const btVector3& a,
+										  const btVector3& b,
+										  const btVector3& c,
+										  btVector3& prj,
+										  btScalar& sqd)
+{
+	const btVector3&	q=cross(b-a,c-a);
+	const btScalar		m2=q.length2();
+	if(m2>SIMD_EPSILON)
+	{
+		const btVector3	n=q/btSqrt(m2);
+		const btScalar	k=dot(a,n);
+		const btScalar	k2=k*k;
+		if(k2<sqd)
+		{
+			const btVector3	p=n*k;
+			if(	(dot(cross(a-p,b-p),q)>0)&&
+				(dot(cross(b-p,c-p),q)>0)&&
+				(dot(cross(c-p,a-p),q)>0))
+			{			
+				prj=p;
+				sqd=k2;
+			}
+			else
+			{
+				ProjectOrigin(a,b,prj,sqd);
+				ProjectOrigin(b,c,prj,sqd);
+				ProjectOrigin(c,a,prj,sqd);
+			}
+		}
+	}
+}
+
+//
+template <typename T>
+static inline T				BaryEval(		const T& a,
+									 const T& b,
+									 const T& c,
+									 const btVector3& coord)
+{
+	return(a*coord.x()+b*coord.y()+c*coord.z());
+}
+//
+static inline btVector3		BaryCoord(	const btVector3& a,
+									  const btVector3& b,
+									  const btVector3& c,
+									  const btVector3& p)
+{
+	const btScalar	w[]={	cross(a-p,b-p).length(),
+		cross(b-p,c-p).length(),
+		cross(c-p,a-p).length()};
+	const btScalar	isum=1/(w[0]+w[1]+w[2]);
+	return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
+}
+
+//
+static btScalar				ImplicitSolve(	btSoftBody::ImplicitFn* fn,
+										  const btVector3& a,
+										  const btVector3& b,
+										  const btScalar accuracy,
+										  const int maxiterations=256)
+{
+	btScalar	span[2]={0,1};
+	btScalar	values[2]={fn->Eval(a),fn->Eval(b)};
+	if(values[0]>values[1])
+	{
+		btSwap(span[0],span[1]);
+		btSwap(values[0],values[1]);
+	}
+	if(values[0]>-accuracy) return(-1);
+	if(values[1]<+accuracy) return(-1);
+	for(int i=0;i<maxiterations;++i)
+	{
+		const btScalar	t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
+		const btScalar	v=fn->Eval(Lerp(a,b,t));
+		if((t<=0)||(t>=1))		break;
+		if(btFabs(v)<accuracy)	return(t);
+		if(v<0)
+		{ span[0]=t;values[0]=v; }
+		else
+		{ span[1]=t;values[1]=v; }
+	}
+	return(-1);
+}
+
+//
+static inline btVector3		NormalizeAny(const btVector3& v)
+{
+	const btScalar l=v.length();
+	if(l>SIMD_EPSILON)
+		return(v/l);
+	else
+		return(btVector3(0,0,0));
+}
+
+//
+static inline btDbvtVolume	VolumeOf(	const btSoftBody::Face& f,
+									 btScalar margin)
+{
+	const btVector3*	pts[]={	&f.m_n[0]->m_x,
+		&f.m_n[1]->m_x,
+		&f.m_n[2]->m_x};
+	btDbvtVolume		vol=btDbvtVolume::FromPoints(pts,3);
+	vol.Expand(btVector3(margin,margin,margin));
+	return(vol);
+}
+
+//
+static inline btVector3			CenterOf(	const btSoftBody::Face& f)
+{
+	return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
+}
+
+//
+static inline btScalar			AreaOf(		const btVector3& x0,
+									   const btVector3& x1,
+									   const btVector3& x2)
+{
+	const btVector3	a=x1-x0;
+	const btVector3	b=x2-x0;
+	const btVector3	cr=cross(a,b);
+	const btScalar	area=cr.length();
+	return(area);
+}
+
+//
+static inline btScalar		VolumeOf(	const btVector3& x0,
+									 const btVector3& x1,
+									 const btVector3& x2,
+									 const btVector3& x3)
+{
+	const btVector3	a=x1-x0;
+	const btVector3	b=x2-x0;
+	const btVector3	c=x3-x0;
+	return(dot(a,cross(b,c)));
+}
+
+//
+static void					EvaluateMedium(	const btSoftBodyWorldInfo* wfi,
+										   const btVector3& x,
+										   btSoftBody::sMedium& medium)
+{
+	medium.m_velocity	=	btVector3(0,0,0);
+	medium.m_pressure	=	0;
+	medium.m_density	=	wfi->air_density;
+	if(wfi->water_density>0)
+	{
+		const btScalar	depth=-(dot(x,wfi->water_normal)+wfi->water_offset);
+		if(depth>0)
+		{
+			medium.m_density	=	wfi->water_density;
+			medium.m_pressure	=	depth*wfi->water_density*wfi->m_gravity.length();
+		}
+	}
+}
+
+//
+static inline void			ApplyClampedForce(	btSoftBody::Node& n,
+											  const btVector3& f,
+											  btScalar dt)
+{
+	const btScalar	dtim=dt*n.m_im;
+	if((f*dtim).length2()>n.m_v.length2())
+	{/* Clamp	*/ 
+		n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;						
+	}
+	else
+	{/* Apply	*/ 
+		n.m_f+=f;
+	}
+}
+
+//
+static inline int		MatchEdge(	const btSoftBody::Node* a,
+								  const btSoftBody::Node* b,
+								  const btSoftBody::Node* ma,
+								  const btSoftBody::Node* mb)
+{
+	if((a==ma)&&(b==mb)) return(0);
+	if((a==mb)&&(b==ma)) return(1);
+	return(-1);
+}
+
+//
+// btEigen : Extract eigen system,
+// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
+// outputs are NOT sorted.
+//
+struct	btEigen
+{
+	static int			system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
+	{
+		static const int		maxiterations=16;
+		static const btScalar	accuracy=(btScalar)0.0001;
+		btMatrix3x3&			v=*vectors;
+		int						iterations=0;
+		vectors->setIdentity();
+		do	{
+			int				p=0,q=1;
+			if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
+			if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
+			if(btFabs(a[p][q])>accuracy)
+			{
+				const btScalar	w=(a[q][q]-a[p][p])/(2*a[p][q]);
+				const btScalar	z=btFabs(w);
+				const btScalar	t=w/(z*(btSqrt(1+w*w)+z));
+				if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ 
+				{
+					const btScalar	c=1/btSqrt(t*t+1);
+					const btScalar	s=c*t;
+					mulPQ(a,c,s,p,q);
+					mulTPQ(a,c,s,p,q);
+					mulPQ(v,c,s,p,q);
+				} else break;
+			} else break;
+		} while((++iterations)<maxiterations);
+		if(values)
+		{
+			*values=btVector3(a[0][0],a[1][1],a[2][2]);
+		}
+		return(iterations);
+	}
+private:
+	static inline void	mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+	{
+		const btScalar	m[2][3]={	{a[p][0],a[p][1],a[p][2]},
+		{a[q][0],a[q][1],a[q][2]}};
+		int i;
+
+		for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
+		for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
+	}
+	static inline void	mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
+	{
+		const btScalar	m[2][3]={	{a[0][p],a[1][p],a[2][p]},
+		{a[0][q],a[1][q],a[2][q]}};
+		int i;
+
+		for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
+		for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
+	}
+};
+
+//
+// Polar decomposition,
+// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
+//
+static inline int			PolarDecompose(	const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
+{
+	static const btScalar	half=(btScalar)0.5;
+	static const btScalar	accuracy=(btScalar)0.0001;
+	static const int		maxiterations=16;
+	int						i=0;
+	btScalar				det=0;
+	q	=	Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
+	det	=	q.determinant();
+	if(!btFuzzyZero(det))
+	{
+		for(;i<maxiterations;++i)
+		{
+			q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
+			const btScalar	ndet=q.determinant();
+			if(Sq(ndet-det)>accuracy) det=ndet; else break;
+		}
+		/* Final orthogonalization	*/ 
+		Orthogonalize(q);
+		/* Compute 'S'				*/ 
+		s=q.transpose()*m;
+	}
+	else
+	{
+		q.setIdentity();
+		s.setIdentity();
+	}
+	return(i);
+}
+
+//
+// btSoftColliders
+//
+struct btSoftColliders
+{
+	//
+	// ClusterBase
+	//
+	struct	ClusterBase : btDbvt::ICollide
+	{
+		btScalar			erp;
+		btScalar			idt;
+		btScalar			margin;
+		btScalar			friction;
+		btScalar			threshold;
+		ClusterBase()
+		{
+			erp			=(btScalar)1;
+			idt			=0;
+			margin		=0;
+			friction	=0;
+			threshold	=(btScalar)0;
+		}
+		bool				SolveContact(	const btGjkEpaSolver2::sResults& res,
+			btSoftBody::Body ba,btSoftBody::Body bb,
+			btSoftBody::CJoint& joint)
+		{
+			if(res.distance<margin)
+			{
+				const btVector3		ra=res.witnesses[0]-ba.xform().getOrigin();
+				const btVector3		rb=res.witnesses[1]-bb.xform().getOrigin();
+				const btVector3		va=ba.velocity(ra);
+				const btVector3		vb=bb.velocity(rb);
+				const btVector3		vrel=va-vb;
+				const btScalar		rvac=dot(vrel,res.normal);
+				const btScalar		depth=res.distance-margin;
+				const btVector3		iv=res.normal*rvac;
+				const btVector3		fv=vrel-iv;
+				joint.m_bodies[0]	=	ba;
+				joint.m_bodies[1]	=	bb;
+				joint.m_refs[0]		=	ra*ba.xform().getBasis();
+				joint.m_refs[1]		=	rb*bb.xform().getBasis();
+				joint.m_rpos[0]		=	ra;
+				joint.m_rpos[1]		=	rb;
+				joint.m_cfm			=	1;
+				joint.m_erp			=	1;
+				joint.m_life		=	0;
+				joint.m_maxlife		=	0;
+				joint.m_split		=	1;
+				joint.m_drift		=	depth*res.normal;
+				joint.m_normal		=	res.normal;
+				joint.m_delete		=	false;
+				joint.m_friction	=	fv.length2()<(-rvac*friction)?1:friction;
+				joint.m_massmatrix	=	ImpulseMatrix(	ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
+					bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
+				return(true);
+			}
+			return(false);
+		}
+	};
+	//
+	// CollideCL_RS
+	//
+	struct	CollideCL_RS : ClusterBase
+	{
+		btSoftBody*		psb;
+		btRigidBody*	prb;
+		void		Process(const btDbvtNode* leaf)
+		{
+			btSoftBody::Cluster*		cluster=(btSoftBody::Cluster*)leaf->data;
+			btSoftClusterCollisionShape	cshape(cluster);
+			const btConvexShape*		rshape=(const btConvexShape*)prb->getCollisionShape();
+			btGjkEpaSolver2::sResults	res;		
+			if(btGjkEpaSolver2::SignedDistance(	&cshape,btTransform::getIdentity(),
+				rshape,prb->getInterpolationWorldTransform(),
+				btVector3(1,0,0),res))
+			{
+				btSoftBody::CJoint	joint;
+				if(SolveContact(res,cluster,prb,joint))
+				{
+					btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
+					*pj=joint;psb->m_joints.push_back(pj);
+					if(prb->isStaticOrKinematicObject())
+					{
+						pj->m_erp	*=	psb->m_cfg.kSKHR_CL;
+						pj->m_split	*=	psb->m_cfg.kSK_SPLT_CL;
+					}
+					else
+					{
+						pj->m_erp	*=	psb->m_cfg.kSRHR_CL;
+						pj->m_split	*=	psb->m_cfg.kSR_SPLT_CL;
+					}
+				}
+			}
+		}
+		void		Process(btSoftBody* ps,btRigidBody* pr)
+		{
+			psb			=	ps;
+			prb			=	pr;
+			idt			=	ps->m_sst.isdt;
+			margin		=	ps->getCollisionShape()->getMargin()+
+				pr->getCollisionShape()->getMargin();
+			friction	=	btMin(psb->m_cfg.kDF,prb->getFriction());
+			btVector3			mins;
+			btVector3			maxs;
+
+			ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume;
+			pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
+			volume=btDbvtVolume::FromMM(mins,maxs);
+			volume.Expand(btVector3(1,1,1)*margin);
+			btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
+		}	
+	};
+	//
+	// CollideCL_SS
+	//
+	struct	CollideCL_SS : ClusterBase
+	{
+		btSoftBody*	bodies[2];
+		void		Process(const btDbvtNode* la,const btDbvtNode* lb)
+		{
+			btSoftBody::Cluster*		cla=(btSoftBody::Cluster*)la->data;
+			btSoftBody::Cluster*		clb=(btSoftBody::Cluster*)lb->data;
+			btSoftClusterCollisionShape	csa(cla);
+			btSoftClusterCollisionShape	csb(clb);
+			btGjkEpaSolver2::sResults	res;		
+			if(btGjkEpaSolver2::SignedDistance(	&csa,btTransform::getIdentity(),
+				&csb,btTransform::getIdentity(),
+				cla->m_com-clb->m_com,res))
+			{
+				btSoftBody::CJoint	joint;
+				if(SolveContact(res,cla,clb,joint))
+				{
+					btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
+					*pj=joint;bodies[0]->m_joints.push_back(pj);
+					pj->m_erp	*=	btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
+					pj->m_split	*=	(bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
+				}
+			}
+		}
+		void		Process(btSoftBody* psa,btSoftBody* psb)
+		{
+			idt			=	psa->m_sst.isdt;
+			margin		=	(psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
+			friction	=	btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
+			bodies[0]	=	psa;
+			bodies[1]	=	psb;
+			btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
+		}	
+	};
+	//
+	// CollideSDF_RS
+	//
+	struct	CollideSDF_RS : btDbvt::ICollide
+	{
+		void		Process(const btDbvtNode* leaf)
+		{
+			btSoftBody::Node*	node=(btSoftBody::Node*)leaf->data;
+			DoNode(*node);
+		}
+		void		DoNode(btSoftBody::Node& n) const
+		{
+			const btScalar			m=n.m_im>0?dynmargin:stamargin;
+			btSoftBody::RContact	c;
+			if(	(!n.m_battach)&&
+				psb->checkContact(prb,n.m_x,m,c.m_cti))
+			{
+				const btScalar	ima=n.m_im;
+				const btScalar	imb=prb->getInvMass();
+				const btScalar	ms=ima+imb;
+				if(ms>0)
+				{
+					const btTransform&	wtr=prb->getInterpolationWorldTransform();
+					const btMatrix3x3&	iwi=prb->getInvInertiaTensorWorld();
+					const btVector3		ra=n.m_x-wtr.getOrigin();
+					const btVector3		va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
+					const btVector3		vb=n.m_x-n.m_q;	
+					const btVector3		vr=vb-va;
+					const btScalar		dn=dot(vr,c.m_cti.m_normal);
+					const btVector3		fv=vr-c.m_cti.m_normal*dn;
+					const btScalar		fc=psb->m_cfg.kDF*prb->getFriction();
+					c.m_node	=	&n;
+					c.m_c0		=	ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
+					c.m_c1		=	ra;
+					c.m_c2		=	ima*psb->m_sst.sdt;
+					c.m_c3		=	fv.length2()<(btFabs(dn)*fc)?0:1-fc;
+					c.m_c4		=	prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
+					psb->m_rcontacts.push_back(c);
+					prb->activate();
+				}
+			}
+		}
+		btSoftBody*		psb;
+		btRigidBody*	prb;
+		btScalar		dynmargin;
+		btScalar		stamargin;
+	};
+	//
+	// CollideVF_SS
+	//
+	struct	CollideVF_SS : btDbvt::ICollide
+	{
+		void		Process(const btDbvtNode* lnode,
+			const btDbvtNode* lface)
+		{
+			btSoftBody::Node*	node=(btSoftBody::Node*)lnode->data;
+			btSoftBody::Face*	face=(btSoftBody::Face*)lface->data;
+			btVector3			o=node->m_x;
+			btVector3			p;
+			btScalar			d=SIMD_INFINITY;
+			ProjectOrigin(	face->m_n[0]->m_x-o,
+				face->m_n[1]->m_x-o,
+				face->m_n[2]->m_x-o,
+				p,d);
+			const btScalar	m=mrg+(o-node->m_q).length()*2;
+			if(d<(m*m))
+			{
+				const btSoftBody::Node*	n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
+				const btVector3			w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
+				const btScalar			ma=node->m_im;
+				btScalar				mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
+				if(	(n[0]->m_im<=0)||
+					(n[1]->m_im<=0)||
+					(n[2]->m_im<=0))
+				{
+					mb=0;
+				}
+				const btScalar	ms=ma+mb;
+				if(ms>0)
+				{
+					btSoftBody::SContact	c;
+					c.m_normal		=	p/-btSqrt(d);
+					c.m_margin		=	m;
+					c.m_node		=	node;
+					c.m_face		=	face;
+					c.m_weights		=	w;
+					c.m_friction	=	btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
+					c.m_cfm[0]		=	ma/ms*psb[0]->m_cfg.kSHR;
+					c.m_cfm[1]		=	mb/ms*psb[1]->m_cfg.kSHR;
+					psb[0]->m_scontacts.push_back(c);
+				}
+			}	
+		}
+		btSoftBody*		psb[2];
+		btScalar		mrg;
+	};
+};
+
+#endif //_BT_SOFT_BODY_INTERNALS_H
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp	(revision 1963)
@@ -0,0 +1,135 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "btSoftRigidCollisionAlgorithm.h"
+#include "btSoftBodyConcaveCollisionAlgorithm.h"
+#include "btSoftSoftCollisionAlgorithm.h"
+
+#include "LinearMath/btPoolAllocator.h"
+
+#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1
+
+btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo)
+:btDefaultCollisionConfiguration(constructionInfo)
+{
+	void* mem;
+
+	mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16);
+	m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+	m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16);
+	m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc;
+	m_swappedSoftRigidConvexCreateFunc->m_swapped=true;
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
+	m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
+
+	mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
+	m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
+	m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
+#endif
+
+	//replace pool by a new one, with potential larger size
+
+	if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool)
+	{
+		int curElemSize = m_collisionAlgorithmPool->getElementSize();
+		///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+
+
+		int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm);
+		int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm);
+		int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm);
+
+		int	collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1);
+		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
+		if (collisionAlgorithmMaxElementSize > curElemSize)
+		{
+			btAlignedFree(m_collisionAlgorithmPool);
+			void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+			m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize);
+		}
+	}
+
+
+
+
+}
+
+btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration()
+{
+	m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_softSoftCreateFunc);
+
+	m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_softRigidConvexCreateFunc);
+
+	m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_swappedSoftRigidConvexCreateFunc);
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+	m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_softRigidConcaveCreateFunc);
+
+	m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(	m_swappedSoftRigidConcaveCreateFunc);
+#endif
+}
+
+///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
+{
+
+	///try to handle the softbody interactions first
+
+	if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE))
+	{
+		return	m_softSoftCreateFunc;
+	}
+
+	///softbody versus convex
+	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return	m_softRigidConvexCreateFunc;
+	}
+
+	///convex versus soft body
+	if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+	{
+		return	m_swappedSoftRigidConvexCreateFunc;
+	}
+
+#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS
+	///softbody versus convex
+	if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE  && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return	m_softRigidConcaveCreateFunc;
+	}
+
+	///convex versus soft body
+	if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE )
+	{
+		return	m_swappedSoftRigidConcaveCreateFunc;
+	}
+#endif
+
+	///fallback to the regular rigid collision shape
+	return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1);
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h	(revision 1963)
@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+class btVoronoiSimplexSolver;
+class btGjkEpaPenetrationDepthSolver;
+
+
+///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration
+class	btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration
+{
+
+	//default CreationFunctions, filling the m_doubleDispatch table
+	btCollisionAlgorithmCreateFunc*	m_softSoftCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_softRigidConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedSoftRigidConvexCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_softRigidConcaveCreateFunc;
+	btCollisionAlgorithmCreateFunc*	m_swappedSoftRigidConcaveCreateFunc;
+
+public:
+
+	btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo());
+
+	virtual ~btSoftBodyRigidBodyCollisionConfiguration();
+
+	///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+};
+
+#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION
+
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,79 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSoftRigidCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "btSoftBody.h"
+///TODO: include all the shapes that the softbody can collide with
+///alternatively, implement special case collision algorithms (just like for rigid collision shapes)
+
+//#include <stdio.h>
+
+btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped)
+: btCollisionAlgorithm(ci),
+//m_ownManifold(false),
+//m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+}
+
+
+btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
+{
+
+	//m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject);
+
+	/*if (m_ownManifold)
+	{
+	if (m_manifoldPtr)
+	m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+	*/
+
+}
+
+
+#include <stdio.h>
+
+void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	//printf("btSoftRigidCollisionAlgorithm\n");
+
+	btSoftBody* softBody =  m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0;
+	btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1;
+
+	softBody->defaultCollisionHandler(rigidCollisionObject);
+
+
+}
+
+btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)resultOut;
+	(void)dispatchInfo;
+	(void)col0;
+	(void)col1;
+
+	//not yet
+	return btScalar(1.);
+}
+
+
+
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,75 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H
+#define SOFT_RIGID_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+class btPersistentManifold;
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+#include "LinearMath/btVector3.h"
+class btSoftBody;
+
+/// btSoftRigidCollisionAlgorithm  provides collision detection between btSoftBody and btRigidBody
+class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm
+{
+	//	bool	m_ownManifold;
+	//	btPersistentManifold*	m_manifoldPtr;
+
+	btSoftBody*				m_softBody;
+	btCollisionObject*		m_rigidCollisionObject;
+
+	///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean
+	bool	m_isSwapped;
+
+public:
+
+	btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+
+	virtual ~btSoftRigidCollisionAlgorithm();
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		//we don't add any manifolds
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false);
+			} else
+			{
+				return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true);
+			}
+		}
+	};
+
+};
+
+#endif //SOFT_RIGID_COLLISION_ALGORITHM_H
+
+
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.cpp	(revision 1963)
@@ -0,0 +1,135 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btSoftRigidDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "btSoftBody.h"
+#include "btSoftBodyHelpers.h"
+
+
+
+
+
+btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+	m_drawFlags			=	fDrawFlags::Std;
+	m_drawNodeTree		=	true;
+	m_drawFaceTree		=	false;
+	m_drawClusterTree	=	false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.Reset();
+
+}
+
+btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
+{
+
+}
+
+void	btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
+
+	for ( int i=0;i<m_softBodies.size();++i)
+	{
+		btSoftBody*	psb= m_softBodies[i];
+
+		psb->predictMotion(timeStep);		
+	}
+}
+
+void	btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+
+	///solve soft bodies constraints
+	solveSoftBodiesConstraints();
+
+	///update soft bodies
+	updateSoftBodies();
+
+}
+
+void	btSoftRigidDynamicsWorld::updateSoftBodies()
+{
+	BT_PROFILE("updateSoftBodies");
+
+	for ( int i=0;i<m_softBodies.size();i++)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->integrateMotion();	
+	}
+}
+
+void	btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
+{
+	BT_PROFILE("solveSoftConstraints");
+
+	if(m_softBodies.size())
+	{
+		btSoftBody::solveClusters(m_softBodies);
+	}
+
+	for(int i=0;i<m_softBodies.size();++i)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->solveConstraints();
+	}	
+}
+
+void	btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body)
+{
+	m_softBodies.push_back(body);
+
+	btCollisionWorld::addCollisionObject(body,
+		btBroadphaseProxy::DefaultFilter,
+		btBroadphaseProxy::AllFilter);
+
+}
+
+void	btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	m_softBodies.remove(body);
+
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void	btSoftRigidDynamicsWorld::debugDrawWorld()
+{
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
+	if (getDebugDrawer())
+	{
+		int i;
+		for (  i=0;i<this->m_softBodies.size();i++)
+		{
+			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+			btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+				if(m_drawFaceTree)		btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
+				if(m_drawClusterTree)	btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
+			}
+		}		
+	}	
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftRigidDynamicsWorld.h	(revision 1963)
@@ -0,0 +1,82 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H
+#define BT_SOFT_RIGID_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "btSoftBody.h"
+
+typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+	btSoftBodyArray	m_softBodies;
+	int				m_drawFlags;
+	bool			m_drawNodeTree;
+	bool			m_drawFaceTree;
+	bool			m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+
+protected:
+
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+	void	updateSoftBodies();
+
+	void	solveSoftBodiesConstraints();
+
+
+public:
+
+	btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+
+	virtual ~btSoftRigidDynamicsWorld();
+
+	virtual void	debugDrawWorld();
+
+	void	addSoftBody(btSoftBody* body);
+
+	void	removeSoftBody(btSoftBody* body);
+
+	int		getDrawFlags() const { return(m_drawFlags); }
+	void	setDrawFlags(int f)	{ m_drawFlags=f; }
+
+	btSoftBodyWorldInfo&	getWorldInfo()
+	{
+		return m_sbi;
+	}
+	const btSoftBodyWorldInfo&	getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+};
+
+#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp	(revision 1963)
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSoftSoftCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "btSoftBody.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/)
+: btCollisionAlgorithm(ci)
+//m_ownManifold(false),
+//m_manifoldPtr(mf)
+{
+}
+
+btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
+{
+}
+
+void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	btSoftBody* soft0 =	(btSoftBody*)body0;
+	btSoftBody* soft1 =	(btSoftBody*)body1;
+	soft0->defaultCollisionHandler(soft1);
+}
+
+btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	//not yet
+	return 1.f;
+}
Index: code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSoftSoftCollisionAlgorithm.h	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H
+#define SOFT_SOFT_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+class btSoftBody;
+
+///collision detection between two btSoftBody shapes
+class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+
+	btSoftBody*	m_softBody0;
+	btSoftBody*	m_softBody1;
+
+
+public:
+	btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+	btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
+
+	virtual ~btSoftSoftCollisionAlgorithm();
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
+		{
+			int bbsize = sizeof(btSoftSoftCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1);
+		}
+	};
+
+};
+
+#endif //SOFT_SOFT_COLLISION_ALGORITHM_H
+
+
Index: code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h
===================================================================
--- code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h	(revision 1963)
+++ code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h	(revision 1963)
@@ -0,0 +1,306 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSparseSdf implementation by Nathanael Presson
+
+#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
+#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+
+// Modified Paul Hsieh hash
+template <const int DWORDLEN>
+unsigned int HsiehHash(const void* pdata)
+{
+	const unsigned short*	data=(const unsigned short*)pdata;
+	unsigned				hash=DWORDLEN<<2,tmp;
+	for(int i=0;i<DWORDLEN;++i)
+	{
+		hash	+=	data[0];
+		tmp		=	(data[1]<<11)^hash;
+		hash	=	(hash<<16)^tmp;
+		data	+=	2;
+		hash	+=	hash>>11;
+	}
+	hash^=hash<<3;hash+=hash>>5;
+	hash^=hash<<4;hash+=hash>>17;
+	hash^=hash<<25;hash+=hash>>6;
+	return(hash);
+}
+
+template <const int CELLSIZE>
+struct	btSparseSdf
+{
+	//
+	// Inner types
+	//
+	struct IntFrac
+	{
+		int					b;
+		int					i;
+		btScalar			f;
+	};
+	struct	Cell
+	{
+		btScalar			d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
+		int					c[3];
+		int					puid;
+		unsigned			hash;
+		btCollisionShape*	pclient;
+		Cell*				next;
+	};
+	//
+	// Fields
+	//
+
+	btAlignedObjectArray<Cell*>		cells;	
+	btScalar						voxelsz;
+	int								puid;
+	int								ncells;
+	int								nprobes;
+	int								nqueries;	
+
+	//
+	// Methods
+	//
+
+	//
+	void					Initialize(int hashsize=2383)
+	{
+		cells.resize(hashsize,0);
+		Reset();		
+	}
+	//
+	void					Reset()
+	{
+		for(int i=0,ni=cells.size();i<ni;++i)
+		{
+			Cell*	pc=cells[i];
+			cells[i]=0;
+			while(pc)
+			{
+				Cell*	pn=pc->next;
+				delete pc;
+				pc=pn;
+			}
+		}
+		voxelsz		=0.25;
+		puid		=0;
+		ncells		=0;
+		nprobes		=1;
+		nqueries	=1;
+	}
+	//
+	void					GarbageCollect(int lifetime=256)
+	{
+		const int life=puid-lifetime;
+		for(int i=0;i<cells.size();++i)
+		{
+			Cell*&	root=cells[i];
+			Cell*	pp=0;
+			Cell*	pc=root;
+			while(pc)
+			{
+				Cell*	pn=pc->next;
+				if(pc->puid<life)
+				{
+					if(pp) pp->next=pn; else root=pn;
+					delete pc;pc=pp;--ncells;
+				}
+				pp=pc;pc=pn;
+			}
+		}
+		//printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
+		nqueries=1;
+		nprobes=1;
+		++puid;	/* TODO: Reset puid's when int range limit is reached	*/ 
+		/* else setup a priority list...						*/ 
+	}
+	//
+	int						RemoveReferences(btCollisionShape* pcs)
+	{
+		int	refcount=0;
+		for(int i=0;i<cells.size();++i)
+		{
+			Cell*&	root=cells[i];
+			Cell*	pp=0;
+			Cell*	pc=root;
+			while(pc)
+			{
+				Cell*	pn=pc->next;
+				if(pc->pclient==pcs)
+				{
+					if(pp) pp->next=pn; else root=pn;
+					delete pc;pc=pp;++refcount;
+				}
+				pp=pc;pc=pn;
+			}
+		}
+		return(refcount);
+	}
+	//
+	btScalar				Evaluate(	const btVector3& x,
+		btCollisionShape* shape,
+		btVector3& normal,
+		btScalar margin)
+	{
+		/* Lookup cell			*/ 
+		const btVector3	scx=x/voxelsz;
+		const IntFrac	ix=Decompose(scx.x());
+		const IntFrac	iy=Decompose(scx.y());
+		const IntFrac	iz=Decompose(scx.z());
+		const unsigned	h=Hash(ix.b,iy.b,iz.b,shape);
+		Cell*&			root=cells[static_cast<int>(h%cells.size())];
+		Cell*			c=root;
+		++nqueries;
+		while(c)
+		{
+			++nprobes;
+			if(	(c->hash==h)	&&
+				(c->c[0]==ix.b)	&&
+				(c->c[1]==iy.b)	&&
+				(c->c[2]==iz.b)	&&
+				(c->pclient==shape))
+			{ break; }
+			else
+			{ c=c->next; }
+		}
+		if(!c)
+		{
+			++nprobes;		
+			++ncells;
+			c=new Cell();
+			c->next=root;root=c;
+			c->pclient=shape;
+			c->hash=h;
+			c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
+			BuildCell(*c);
+		}
+		c->puid=puid;
+		/* Extract infos		*/ 
+		const int		o[]={	ix.i,iy.i,iz.i};
+		const btScalar	d[]={	c->d[o[0]+0][o[1]+0][o[2]+0],
+			c->d[o[0]+1][o[1]+0][o[2]+0],
+			c->d[o[0]+1][o[1]+1][o[2]+0],
+			c->d[o[0]+0][o[1]+1][o[2]+0],
+			c->d[o[0]+0][o[1]+0][o[2]+1],
+			c->d[o[0]+1][o[1]+0][o[2]+1],
+			c->d[o[0]+1][o[1]+1][o[2]+1],
+			c->d[o[0]+0][o[1]+1][o[2]+1]};
+		/* Normal	*/ 
+#if 1
+		const btScalar	gx[]={	d[1]-d[0],d[2]-d[3],
+			d[5]-d[4],d[6]-d[7]};
+		const btScalar	gy[]={	d[3]-d[0],d[2]-d[1],
+			d[7]-d[4],d[6]-d[5]};
+		const btScalar	gz[]={	d[4]-d[0],d[5]-d[1],
+			d[7]-d[3],d[6]-d[2]};
+		normal.setX(Lerp(	Lerp(gx[0],gx[1],iy.f),
+			Lerp(gx[2],gx[3],iy.f),iz.f));
+		normal.setY(Lerp(	Lerp(gy[0],gy[1],ix.f),
+			Lerp(gy[2],gy[3],ix.f),iz.f));
+		normal.setZ(Lerp(	Lerp(gz[0],gz[1],ix.f),
+			Lerp(gz[2],gz[3],ix.f),iy.f));
+		normal		=	normal.normalized();
+#else
+		normal		=	btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
+#endif
+		/* Distance	*/ 
+		const btScalar	d0=Lerp(Lerp(d[0],d[1],ix.f),
+			Lerp(d[3],d[2],ix.f),iy.f);
+		const btScalar	d1=Lerp(Lerp(d[4],d[5],ix.f),
+			Lerp(d[7],d[6],ix.f),iy.f);
+		return(Lerp(d0,d1,iz.f)-margin);
+	}
+	//
+	void					BuildCell(Cell& c)
+	{
+		const btVector3	org=btVector3(	(btScalar)c.c[0],
+			(btScalar)c.c[1],
+			(btScalar)c.c[2])	*
+			CELLSIZE*voxelsz;
+		for(int k=0;k<=CELLSIZE;++k)
+		{
+			const btScalar	z=voxelsz*k+org.z();
+			for(int j=0;j<=CELLSIZE;++j)
+			{
+				const btScalar	y=voxelsz*j+org.y();
+				for(int i=0;i<=CELLSIZE;++i)
+				{
+					const btScalar	x=voxelsz*i+org.x();
+					c.d[i][j][k]=DistanceToShape(	btVector3(x,y,z),
+						c.pclient);
+				}
+			}
+		}
+	}
+	//
+	static inline btScalar	DistanceToShape(const btVector3& x,
+		btCollisionShape* shape)
+	{
+		btTransform	unit;
+		unit.setIdentity();
+		if(shape->isConvex())
+		{
+			btGjkEpaSolver2::sResults	res;
+			btConvexShape*				csh=static_cast<btConvexShape*>(shape);
+			return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
+		}
+		return(0);
+	}
+	//
+	static inline IntFrac	Decompose(btScalar x)
+	{
+		/* That one need a lot of improvements...	*/
+		/* Remove test, faster floor...				*/ 
+		IntFrac			r;
+		x/=CELLSIZE;
+		const int		o=x<0?(int)(-x+1):0;
+		x+=o;r.b=(int)x;
+		const btScalar	k=(x-r.b)*CELLSIZE;
+		r.i=(int)k;r.f=k-r.i;r.b-=o;
+		return(r);
+	}
+	//
+	static inline btScalar	Lerp(btScalar a,btScalar b,btScalar t)
+	{
+		return(a+(b-a)*t);
+	}
+
+
+
+	//
+	static inline unsigned int	Hash(int x,int y,int z,btCollisionShape* shape)
+	{
+		struct btS
+		{ 
+			int x,y,z;
+			void* p;
+		};
+
+		btS myset;
+
+		myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
+		const void* ptr = &myset;
+
+		unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
+
+
+		return result;
+	}
+};
+
+
+#endif
Index: code/branches/physics/src/bullet/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,5 @@
+if (CMAKE_SIZEOF_VOID_P MATCHES "8")
+  SUBDIRS( BulletSoftBody BulletCollision BulletDynamics LinearMath )
+else (CMAKE_SIZEOF_VOID_P MATCHES "8")
+  SUBDIRS( BulletMultiThreaded BulletSoftBody BulletCollision BulletDynamics LinearMath )
+endif (CMAKE_SIZEOF_VOID_P MATCHES "8")
Index: code/branches/physics/src/bullet/ChangeLog
===================================================================
--- code/branches/physics/src/bullet/ChangeLog	(revision 1963)
+++ code/branches/physics/src/bullet/ChangeLog	(revision 1963)
@@ -0,0 +1,676 @@
+Bullet Continuous Collision Detection and Physics Library
+Primary author and maintainer: Erwin Coumans
+
+Todo: update changelog from April - July 2008
+See http://code.google.com/p/bullet/source/list for more complete log in Subversion
+
+2008 October 10
+	- Moved aabb to btBroadphaseProxy, improves rayTest dramatically. Further raytest improvements using the broadphase acceleration structures are planned
+
+2008 October 3
+	- Add support for autoconf automake
+	./autogen.sh and ./configure will create both Makefile and Jamfile. CMake and autogenerated Visual Studio projectfiles remain supported too.
+	- Improved ColladaConverter: plane shape export, and callback for shape construction to allow deletion of memory
+	
+2008 Sept 30
+	- Improved Soft Body support, fixed issues related to soft body colliding against concave triangle meshes
+	- Shared more code between regular version and SPU/BulletMultiThreaded, in particular GJK/EPA
+
+2008 Sept 28
+	- Fixed rotation issues in Dynamic Maya Plugin
+
+2008 Sept 11
+	- Enable CCD motion clamping for btDiscreteDynamicsWorld, to avoid tunneling. A more advanced solution will be implemented in btContinuousDynamicsWorld.
+
+2008 Sept 7
+	- Add btScaledBvhTriangleMeshShape, to allow re-use of btBvhTriangleMeshShape of different sizes, without copying of the BVH data.
+
+2008 Sept 5
+	- Enabled Demos/ForkLiftDemo
+	Thanks Roman Ponomarev.
+
+2008 Sept 4
+	- Added btCudaBroadphase in Extras/CUDA: some research into accelerating Bullet using CUDA.
+	Thanks to the particle demo from the NVidia CUDA SDK.
+	
+2008 Sept 3
+	- Several bug fixes and contributions related to inertia tensor, memory leaks etc.
+	Thanks to Ole K.
+
+2008 Sept 1
+	- Updated CDTestFramework, with latest version of OPCODE Array SAP. See Extras/CDTestFramework
+	Thanks to Pierre Terdiman for the update
+
+2008 August 25
+	- Walt Disney Studios contributes their in-house Maya Plugin for simulating Bullet physics, with options for other engines such as PhysBam or PhysX.
+	Thanks to Nicola Candussi and Arthur Shek
+	
+2008 August 14
+	- Improved performance for btDbvtBroadphase, based on dual dynamic AABB trees (one for static, one for dynamic objects, where objects can move from one to the other tree)
+	Thanks to Nathanael Presson again, for all his work.
+	
+2008 July 31
+	- Added Havok .hkx to COLLADA Physics .dae converter patch+information
+	- Fix btSubsimplexConvexCast
+	Thanks to Nacho, http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2422)
+	- Fix in rendering, GL_STENCIL
+	- btTriangleIndexVertexArray indices should be unsigned int/unsigned short int,
+	- Made InternalProcessAllTriangles virtual, thanks to
+	Both thank to Fullmetalcoder, http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2401
+	- clamp impulse for btPoint2PointConstraint
+	Thanks to Martijn Reuvers, http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2418
+	- Free memory of bvh, pass in scaling factor (optional)
+	Thanks to Roy Eltham, http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2375
+
+2008 July 27
+
+btDbvtBroadphase:
+	- Fixed a performance issues reported by 'reltham'
+	- Added btDbvtBroadphase::optimize() for people who want good performances right
+away or don't do dynamics.
+	- fixed compilation issues when DBVT_BP_PROFILE was set.
+btSoftBody:
+	- Fixed singular matrix issues related to polar decomposition (flat meshes).
+DemoApplication:
+	- Shadows (enable/disable through 'g' or DemoApplication::setShadows(bool)).
+	- Texture can be enable/disable through 'u'
+CDFramework:
+	- fixed compilation issues.
+	All thanks to Nathanael Presson
+
+2008 July 10
+	- Added btMultimaterialTriangleMeshShape and MultiMaterialDemo
+	Thanks to Alex Silverman for the contribution
+
+2008 June 30
+	- Added initial support for kinematic character controller
+	Thanks to John McCutchan
+
+2008 April 14
+	- Added ray cast support for Soft Bodies
+	Thanks to Nathanael Presson for the contribution
+	
+2008 April 9
+	- Cleanup of Stan Melax ConvexHull, removed Extras/ConvexHull, moved sources into LinearMath/BulletCollision
+
+2008 April 4
+	- Added btSliderConstraint and demo
+	Thanks Roman Ponomarev
+	
+2008 April 3
+	- Fixed btMinkowskiSumShape, and added hitpoint to btSubsimplexConvexCast
+
+2008 April 2
+	- Added Extras/CdTestFrameWork
+	Thanks Pierre Terdiman
+
+2008 April 1
+	- Added posix thread (pthread) support
+	Thanks Enrico
+
+2008 March 30
+	- Added Soft Body, cloth, rope and deformable volumes, including demos and interaction
+	Thanks Nathanael Presson for this great contribution
+	
+	2008 March 17
+	- Improved BulletColladaConverter
+	Thanks John McCutchan
+
+2008 March 15
+	- btMultiSapBroadphase in a working state. Needs more optimizations to be fully useable.
+	- Allow btOptimizedBvh to be used for arbitrary objects, not just triangles
+	- added quicksort to btAlignedObjectArray
+	- removed btTypedUserInfo, added btHashMap
+
+2008 March 30
+	- Moved quickstep solver and boxbox into Bullet/src folder
+	Thanks Russell L. Smith for permission to redistribute Open Dynamics Engine quickstep and box-box under the ZLib license	
+
+2008 Feb 27
+	- Added initial version for Character Control Demo
+	- Applied fixes to IBM Cell SDK 3.0 build makefiles
+	Thanks Jochen and mojo for reporting/providing patch: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1922
+
+2008 Feb 8
+	- Bugfixes in ConvexCast support against the world.
+	Thanks to Isgmasa for reporting/providing fix: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1823
+
+2008 Feb 6
+	- Added btCapsuleShapeX and btCapsuleShapeZ for capsules around X and Z axis (default capsule is around Y)
+	
+2008 Feb 3
+	- Added btTypedUserInfo, useful for serialization
+
+2008 Jan 31
+	- Add support for 16 and 32-bit indices for SPU / BulletMultiThreaded version.
+
+2008 Jan 29
+	- Added COLLADA Physics export/serialization/snapshot from any Bullet btDynamicsWorld. Saving the physics world into a text .xml file is useful for debugging etc.
+
+2008 Jan 23
+	- Added Stan Melax Convex Hull utility library in Extras/ConvexHull. This is useful to render non-polyhedral convex objects, and to simplify convex polyhedra.
+	
+2008 Jan 14
+	- Add support for batch raycasting on SPU / BulletMultiThreaded
+	
+2007 Dec 16
+	- Added btRigidBodyConstructionInfo, to make it easier to set individual setting (and leave other untouched) during rigid body construction.
+	Thanks Vangelis Kokkevis for pointing this out.
+	- Fixed memoryleak in the ConstraintDemo and Raytracer demo.
+	- Fixed issue with clearing forces/gravity at the end of the stepSimulation, instead of during internalSingleStepSimulation.
+	Thanks chunky for pointing this out: http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1780
+	- Disabled additional damping in rigid body by default, but enable it in most demos. Set btRigidBodyConstructionInfo m_additionalDamping to true to enable this.
+	- Removed obsolete QUICKPROF BEGIN/END_PROFILE, and enabled BT_PROFILE. Profiling is enabled by default (see Bullet/Demos/OpenGL/DemoApplication.cpp how to use this).
+	User can switch off profiling by enabling define BT_NO_PROFILE in Bullet/src/btQuickprof.h.
+
+2007 Dec 14
+	- Added Hello World and BulletMultiThreaded demos
+	- Add portable version of BulletMultiThreaded, through SequentialThreadSupport (non-parallel but sharing the same code-path)
+	- Add Cmake support for AllBulletDemos
+	
+
+2007 Dec 11
+	- Moved the 'btRigidBody::clearForce' to the end of the stepSimulation, instead of in each substep.
+	See discussion http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1601
+	- Added btConvexPlaneCollisionAlgorithm, makes planes perform better, and prevents tunneling
+	Thanks Andy O'Neil for reporting the performance/functionality issue
+	- Fixes for IBM Cell SDK 3.0
+	Thanks to Jochen Roth for the patch.
+
+2007 Dec 10
+	- Fixes in btHeightfieldTerrainShape
+	Thanks to Jay Lee for the patch.
+
+2007	Dec 9
+	- Only update aabb of active objects
+	Thanks Peter Tchernev for reporting (http://bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1764 )
+	- Added workaround to compile libxml under Visual Studio 2008 Beta 2
+	- Make glui compile under MSVC 9.0 beta (vsnprintf is already defined)
+
+2007 Dec 6
+	- Added DynamicControlDemo, showing dynamic control through constraint motors
+		Thanks to Eddy Boxerman
+	- Add support for generic concave shapes for convex cast.
+	- Added convex cast query to collision world.
+	- Added workaround for OpenGL bug in Mac OS X 10.5.0 (Leopard)
+	- Added concave raycast demo
+		All above thanks to John McCutchan (JMC)
+	- Fixed issues that prevent Linux version to compile. 
+	Thanks to Enrico for reporting and patch, see
+	- Fixed misleading name 'numTriangleIndices' into 'numTriangles'
+	Thanks Sean Tasker for reporting:
+
+2007 Nov 28:
+	- Added raycast against trianglemesh. Will be extended to object cast soon.
+	Thanks John McCutchan (JMC)
+	- make getNumPoints const correct, add const getPoints(). 
+	Thanks Dirk Gregorius
+	- Bugfix: allow btCollisionObjects (non-btRigidBody) to interact properly with btRigidBody for cache-friendly btSequentialImpulseConstraintSolver.
+	Thanks Andy O'Neil for pointing this out.
+	- Bugfix: don't fail if spheres have identical center, use arbitrary separating normal (1,0,0)
+	Thanks Sean Tasker for reporting! http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1681
+
+
+2007, November 20
+	- Added hierarchical profiling
+	- Fixed memory leak in btMultiSapBroadphase,
+	- Fixed hash function (typo, should use 2 proxies)
+	Thanks to Stephen (shatcher) for reporting and fixes! http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1696
+
+2007 Nov 11
+	- Fixed parallel solver (BulletMultiThreaded) friction issue
+	- Terminate Win32 Threads when closing the CcdPhysicsDemo (when USE_PARALLEL_SOLVER/USE_PARALLEL_DISPATCHER is defined)
+
+2007 Nov 6
+	- Added support for 16-bit indices for triangle meshes
+	- Added support for multiple mesh parts using btBvhTriangleMeshShape.
+	Thanks to Tim Johansson
+	
+2007 Oct 22
+	- All memory allocations go through btAlignedAlloc/btAlignedFree. User can override this to verify memory leaks
+	- added a few more demos to AllBulletDemos
+	- fix for one of the constructors of btHingeConstraint
+	Thanks Marcus Hennix
+	
+2007 Oct 20
+	- included glui, a GLUT/OpenGL based toolkit for some graphical user elements
+	Removed dynamic_cast from glui, to allow linkage without rtti
+	- added Box2D framework using glui, allowing all demos to run within one executable
+	Thanks Erin Catto for the FrameWork skeleton (http://www.box2d.org)
+
+2007 Ocy 17
+	- Allow user to pass in their own memory (stack and pool) allocators, through collisionConfiguration. See demos how to use this
+
+2007 Oct 14
+	- Included working version of Cell SPU parallel optimized version for Libspe2 SPU task scheduler.
+	This version compiles and runs on Playstation 3 Linux and IBM CellBlade, see BulletSpuOptimized.pdf for build instructions
+	(Official Playstation 3 developers can request a SPURS version through Sony PS3 Devnet.)
+	Thanks to IBM 'Extreme Blue' project for the contribution
+	http://www-913.ibm.com/employment/us/extremeblue/ 
+	Thanks Minh Cuong Tran, Benjamin Hoeferlin, Frederick Roth and Martina Huellmann
+	for various contributions to get this initial Libspe2 parallel version up and running.
+
+2007 Oct 13
+	- made 'btCollisionShape::calculateLocalInertia' const
+	Thanks to cgripeos, see http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1514
+	- applied a large patch to remove warnings
+	Thanks to Enrico, see http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1568
+	- removed SSE includes, added #incude <string.h> for memset in Extras/quickstep, thanks Eternl Knight
+
+2007 Oct 11
+	- added Hashed Overlapping Pair Cache, recommended by Pierre Terdiman. It works like a charm, thanks Pierre and Erin Catto (code from Box2D)
+	- modified some margins inside btBoxShape, btCylinderShape and btSphereShape
+	- added cone debug rendering (for cones with x, y and z up-axis)
+	- added improvements for optional Extra/quickstep, thanks to Remotion
+	- some performance improvements for Bullet constraint solver
+
+2007 Sept 28
+	- upgraded GIMPACT to version 0.3
+	Thanks to Francisco Leon
+
+2007 Sept 27
+	- added contribution from IBM Extreme Blue project for Libspe2 support. This allow to execute BulletMultiThreaded on Cell SPU under PS3 Linux and Cell Blade. See http://www-913.ibm.com/employment/us/extremeblue
+	Thanks to Minh Cuong Tran, Frederick Roth, Martina Heullmann and Benjamin Hoeferlin.
+
+2007 Sept 13
+	- Improved btGenericD6Constraint. It can be used to create ragdolls (similar to the new btConeTwistConstraint). See GenericJointDemo
+	- Added support for Bullet constraints in the optional Extras/quickstep ODE solver. See CcdPhysicsDemo, enable #COMPARE_WITH_QUICKSTEP and add libquickstep to the dependencies.
+	For both patches/improvements thanks Francisco Leon/projectileman
+		
+2007 Sept 10
+	- removed union from btQuadWordStorage, it caused issues under certain version of gcc/Linux
+
+2007 Sept 10
+	- Reverted constraint solver, due to some issues. Need to review the recent memory allocation changes.
+	- Fixed issue with kinematic objects rotating at low speed: quaternion was de-normalized, passing value > 1 into acosf returns #IND00 invalid values
+	- 16 byte memory alignment for BVH serialization
+	- memory cleanup for btPoolAllocator
+
+2007 Sept 9
+	- Added serialization for BVH/btBvhTriangleMeshShape, including endian swapping. See ConcaveDemo for an example.
+	Thanks to Phil Knight for the contribution.
+	- Fixed issues related to stack allocator/compound collision algorithm
+	Thanks Proctoid, http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=18&t=1460
+	- Increase some default memory pool settings, and added a fallback for the constraints solver to use heap memory
+	- Removed accidential testing code in btScalar.h related to operator new.	
+	- Enable btAxis3Sweep and bt32BitAxis3Sweep to be linked in at the same time, using template
+	
+2007 Sept 7
+	- Replaced several dynamic memory allocations by stack allocation and pool allocations
+	- Added branch-free quantized aabb bounding box overlap check, works better on Playstation 3 and XBox 360
+	Thanks to Phil Knight. Also see www.cellperformance.com for related articles
+	- Collision algorithms and settings for the memory/stack allocator can be done using btDefaultCollisionConfiguration
+	This is an API change. See demos how to modify existing implementations with a one-liner.
+	- Register several collision algorithms by default (sphere-sphere, sphere-box, sphere-triangle)
+	- Use other traveral method for BVH by default, this improves triangle mesh collision performance.
+	
+2007 Aug 31
+	- fixed MSVC 6 build
+	Thanks Proctoid, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1375
+	- fixed double precision build issues
+	Thanks Alex Silverman, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1434
+
+2007 Aug 24
+	- fixed bug in btMatrix3x3::transposeTimes(const btMatrix3x3& m) const. Luckily it wasn't used in core parts of the library (yet).
+	Thanks to Jay Lee
+	
+2007 Aug 15
+	- fixed bug in Extras/GIMPACT 0.2 related to moving triangle meshes
+	Thanks Thomas, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1368
+
+2007 Aug 14
+	- added parallel constraint solver. Works on Playstation 3 Cell SPU and multi core (Win Threads on PC and XBox 360).
+	  See Extras/BulletMultiThreaded for SpuSolverTask subfolder and SpuParallelSolver.cpp
+	  Thanks Marten Svanfeldt (Starbreeze Studios)	
+	- fixed some bugs related to parallel collision detection (Extras/BulletMultiThreaded)
+	  Thanks Marten Svanfeldt (Starbreeze Studios)	
+	
+2007 Aug 2
+	- added compound and concave-convex (swapped) case for BulletMultiThreaded collision detection, thanks to Marten Svanfeldt
+	- refactored broadphase and overlapping pair cache. This allows performance improvement by combining multiple broadphases. This helps add/remove of large batches of objects and large worlds. See also Pierre Terdiman forum topic:
+	http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
+
+
+2007 July 27
+	- added Ragdoll Demo
+		Thanks to Marten Svanfeldt (Starbreeze Studios)
+
+	- added Vector Math library for SIMD 3D graphics linear algebra (vector, matrix, quaternion)
+		See Bullet/Extras/vectormathlibrary
+		Supports SIMD SSE, PowerPC PPU and Cell SPU (including PS3 Linux and CellBlade), as well as generic portable scalar version
+		Will be used to improve BulletMultiThreaded performance
+		Open Sourced by Sony Computer Entertainment Inc. under the new BSD license
+	- added SIMD math library
+		4-way SIMD for common math functions like atan2f4, cosf4, floorf4, fabsf4, rsqrtf4 etc. Used by Vector Math library under PPU and SPU.
+		Supports PowerPC (PPU) and Cell SPU, including PS3 Linux and CellBlade.
+		See Bullet/Extras/simdmathlibrary
+		Open sourced by Sony Computer Entertainment Inc. under the new BSD license
+
+	
+2007 July 25
+	- added several patches: per-rigidbody sleeping threshold. added Assert to prevent deletion of rigidbody while constraints are still pointing at it
+		Thanks to Marten Svanfeldt (Starbreeze Studios)
+	
+2007 July 13
+	- fixed relative #include paths again. We can't use "../" relative paths: some compilers choke on it (it causes extreme long paths)
+		Within the libraries, we always need to start with "BulletCollision/" or "BulletDynamics/ or "LinearMath/"
+
+2007 July 10
+	- Updated Bullet User Manual
+
+2007 July 5
+	- added btConeTwistConstraint, especially useful for ragdolls. See Demos/RagdollDemo
+		Thanks to Marten Svanfeldt (Starbreeze Studios)
+
+2007 June 29
+	- btHeightfieldTerrainShape: Added heightfield support, with customizations
+	- Upgraded to GIMPACT 0.2, see Extras/GIMPACT and MovingConcaveDemo
+	- Several patches from Marten Svanfeldt (Starbreeze Studios)
+		Improved collision filtering (in broadphase and rigidbody)
+		Improved debug rendering
+		Allow to set collision filter group/mask in addRigidBody
+		
+		
+2007 June 15
+	- Changed btAlignedObjectArray to call copy constructor/replacement new for duplication, rather then assignment operator (operator=).
+
+2007 June 11
+	- Added multi-threading. Originally for Playstation 3 Cell SPU, but the same code can run using Win32 Threads using fake DMA transfers (memcpy)
+		Libspe2 support for Cell Blade / PS3 Linux is upcoming
+		See Extras/BulletMultiThreaded. Usage: replace btCollisionDispatcher by btSpuGatheringCollisionDispatcher
+
+	- Added managed Bullet library, entirely rewritten in C# for Windows and XBox 360 XNA
+		See Extras/BulletX
+		Thanks to KleMiX, aka Vsevolod Klementjev
+
+2007 May 31
+	- sign-bit went wrong in case of 32-bit broadphase, causing quantization problems. 
+		Thanks DevO for reporting.
+
+2007 May 23
+	- Fixed quantization problem for planar triangle meshes in btOptimizedBvh
+		Thanks Phil Knight for reporting and helping to fix this bug.
+		
+2007 May 20
+	- btAxisSweep3: Fixed a bug in btAxisSweep3 (sweep and prune) related to object removal. Only showed up when at least one btStaticPlaneShape was inserted.
+		Thanks tbp for more details on reproducing case.
+	- btAxisSweep3: Fixed issue with full 32bit precision btAxisSweep3 (define BP_USE_FIXEDPOINT_INT_32), it used only 0xffff/65536 for quantization instead of full integer space (0xffffffff)
+	- btRaycastVehicle: Added 'getForwardVector' and getCurrentSpeedKmHour utility functions
+	- Fixed local scaling issues (btConvexTriangleMeshShape, btBvhTriangleMeshShape, removed scaling from btMatrix3x3). 
+		Thanks Volker for reporting!
+	- Added second filename search, so that starting BspDemo and ConvexDecompositionDemo from within Visual Studio (without setting the starting path) still works
+
+2007 April 22
+	- Added braking functionality to btRaycastVehicle
+	- Removed tons of warnings, under MSVC 2005 compilation in -W4
+	
+2007 March 21
+	- Fixed issues: comma at end of enum causes errors for some compilers
+	- Fixed initialization bug in LocalRayResult (	m_localShapeInfo(localShapeInfo) )
+	
+2007 March 20
+	- Added refit tree to quantized stackless tree, and updated ConcaveDemo as example.
+	
+2007 March 17
+	- Added constraint solver optimizations, avoiding cross products during iterations, and gather rigidbody/constraint info in contiguous memory (btSolverBody/btSolverConstraint)
+	- These optimizations don't give large benefit yet, but it has good potential. Turned on by default. Can be switched off using solver->setSolverMode(SOLVER_RANDMIZE_ORDER).
+	- Enabled anti-jitter for rigid bodies. This is experimental, and can be switched off by setting a global (it is experimental so no proper interface) gJitterVelocityDampingFactor = 1.0;
+	- Fixed bug in islandmanifold.heapSort(btPersistentManifoldSortPredicate()); , thanks Noehrgel for reporting this (affected Sun Solaris)
+	
+2007 March 12
+	- Added compile-time toggle between on 16-bit and 32-bit fixed-point SAP broadphase. 
+		This allows the number of bodies to exceed 32767
+	- Enable useQuantizedAabbCompression on btTriangleMesh, see ColladaDemo
+	
+2007 March 8
+	- Fixed bug in constraint/island sorting (caused by replacing STL by dedicated btAlignedObjectArray with heapSort)
+		Thanks Clemens Unterkofler for pointing this out!
+
+2007 March 6
+	- removed STL from the Bullet library: replace std::vector by btAlignedObjectArray. Also removed the std::set for overlapping pair set, and turned it into an overlapping pair array. The SAP only adds objects, never removed. Removal is postponed for during traversal of overlapping pairs (duplicates and non-overlapping pairs are removed during that traversal).
+	- added heap sort and binary search/linear search to btAlignedObjectArray
+	- fixed wrong cast, thanks Hamstray, http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1015
+
+
+2007 Feb 25
+	- Improved performance of convex collision shapes, cache local AABB instead of recomputation. This fixes issue with very slow performance in larger .bsp levels
+	
+2007 Feb 24
+	- Added compressed/quantized AABB tree, 16 bytes per node, while supporting 32-bit (triangle) indices. 
+	  Should be faster and smaller then original version (quantized aabb check is done in integer space)
+		Original aabb tree nodes are still supported. They are 44 bytes, with full floating point precision and additional subPart index.
+	- added meter-unit scaling support in ColladaConverter.cpp
+
+2007 Feb 21
+	- Build system: updated bullet.pc.in library names
+	- Updated EPA comparison integration (missing parameter)
+	
+2007 Jan 04
+	- fixed optimized AABB tree building: in some cases the tree building fails due to unbalanced trees, which generated stack overflow
+	
+2006 Dec 15
+	- added contribution to allow double precision collision detection/dynamics. Define BT_USE_DOUBLE_PRECISION in your project and libraries that include Bullet
+
+2006 Dec 14
+	- merged contact and non-contact constraint solving into one loop, will improve stability of jointed bodies during collisions
+	- added first draft for hingeConstraint motor
+	
+2006 Dec 8, Erwin Coumans
+	- preparation for SIMD: added btAlignedAllocator and btAlignedObjectArray, to replace stl std::vector, same interface, but compatible with 16 byte alignment
+	- cleaned up dependencies in autogenerated msvc projectfiles
+	- aligned btVector3 on 16 bytes boundary, under win32. see if developers will come up with problems
+
+2006 Dec 04, Erwin Coumans
+	Added btNearCallback. This is similar to Open Dynamics Engine (ODE) dNearCallback, but important differences:
+	- contact points are persistent (lifetime more then one frame, for warmstarting/incremental contact point management)
+	- continuous collision detection, time of impact
+	Added btRigidBody::isInWorld(), returns true if btRigidBody is inside a btCollisionWorld/btDynamicsWorld derived class
+	Added angularFactor to btRigidbody, this helps some character control (no angular impulse applied)
+
+
+2006 Nov 28
+	Moved StackAlloc from EPA into LinearMath/btStackAlloc
+	renamed internal class ConcaveShape into btConcaveShape
+	added btHeightfieldTerrainShape (not completed yet)
+		
+2006 Nov 15 Nathanael Presson
+	Added EPA penetration depth algorithm, Expanding Polytope Algorithm
+	Added Pierre Terdiman penetration depth comparison/test DEMO
+	Fixed Bullet's Minkowski sampling penetration depth solver
+	Contributed by Nathanael Presson
+	
+2006 Nov 11 Francisco León Nájera
+	Added GIMPACT trimesh collision detection: concave versus concave,
+	Contributed by Francisco León Nájera
+
+2006 Nov 2
+	Minor refactoring: btCollisionObject changes from struct into class, added accessor methods
+	Force use of btMotionState to synchronize graphics transform, disabled old btRigidBody constructor that accepts btTransform
+	Renamed treshold into threshold throughout the code
+	
+2006 Oct 30
+	Enable decoupling of physics and graphics framerate using interpolation and internal fixed timestep, based on btMotionState
+	Enabled raycast vehicle demo (still needs tuning)
+	Refresh contact points, even when they are already persistent.
+	Fixed debugDraw colors (thanks pc0de for reporting)
+	Use Dispatcher in ConcaveConvexCollisionAlgorithm (so it uses the registered collision algorithm, not hardcoded convexconcave)
+	Improved performance of constraint solver by precalculating the cross product/impulse arm
+	Added collision comparison code: ODE box-box, also sphere-triangle
+	Added safety check into GJK, and an assert for AABB's that are very large
+	Fixed kinematic support (deriving velocities for animated objects)
+	Updated comparison/optional quickstep solver in Extras
+	UserCollisionAlgorithm demonstrates btTriangleMesh usage (easier trimesh compared to index array version)
+	Removed scaling from btTransform (we only want to deal with rigid transforms)
+	
+2006 Oct 4
+	Fixed minor leak in btOptimizeBVH
+	Cleanup of btRigidBody construction
+	added getW() in btQuaternion
+	assert when setLinearVelocity is called on btRigidBody
+	renamed projectfile library from collada-dom to colladadom (to make VC6 happy)
+	
+2006 Sept 27
+	Big Refactoring: renamed and moved files, create a replacement for CcdPhysicsEnvironment/CcdPhysicsController.
+	All Bullet classes in LinearMath, BulletCollision and BulletDynamics start with bt, and methods start with lowercase.
+	Moved classes into src folder, which is the only include folder needed.
+	Added 2 headerfiles in src: btBulletCollisionCommon.h and btBulletDynamicsCommon.h
+
+2006 Sept 23
+	Fixed 2 bugs, causing crashes when removing objects. Should do better unit-testing. UnionFind and 3D SAP were involved.
+	
+2006 Sept 19
+	Allow programmable friction and contact solver model. User can register their own functions for several interaction types.
+	Improved performance, and removed hardcoded maximum overlaps (switched from C-array to stl::set)
+	
+2006 Sept 16
+	Added Bullet 2.0  User Manual
+	Allow registration of custom user collision algorithms
+
+2006 Sept 10
+	Started cleaning up demos
+	
+2006 Sept 4
+	Fixed concave collision bug (caused instability/missing collisions in meshes/compounds)
+	Fixed memoryleak in OptimizedBvh, added RayTestSingle to CollisionWorld
+	Prepared for VehicleDemo
+	Increased Performance (island generation for sleeping objects took too much time)
+	Better COLLADA 1.4.1 physics conformance in ColladaDemo
+
+2006 August 11
+	Added Quake BspDemo
+	Improved CCD for compound and non-convex objects
+
+2006 August 10
+	Added per-triangle material (friction/restitution) support for non-convex meshes. See ConcaveDemo for usage.
+
+2006 August 9
+	Added CMake support (see http://cmake.org)
+	This can autogenerate makefiles, projectfiles cross platform (including MacOS X Xcode )
+	Just run cmake . in the root folder and it will autogenerate build files
+	
+2006 July 26 Erwin Coumans
+	Upgraded to COLLADA-DOM 1.4.1, latest SVN version
+	ColladaDemo can export snapshots to .dae
+	
+2006 July 24 Erwin Coumans
+	Added Compound CollisionShape support
+	(this is still low performance -> requires stackless tree-versus-tree traversal for better performance)
+
+2006 July 15 Erwin Coumans
+	Added initial support for Parallel execution (collision detection, constraint solving)
+	See ParallelPhysicsEnvironment in Extras\PhysicsInterface\CcdPhysics
+	
+2006 July 10 Erwin Coumans
+	Added MacOS X support (some build issues mainly)
+		
+2006 July 5 Erwin Coumans
+	Improved COLLADA 1.4 physics import, both COLLADA-DOM and FCollada
+
+2006 June 29 Erwin Coumans
+	Refactoring of the broadphase
+	Moved some optional files to Extras: Algebraic ccd and EPA, quickstep
+	Moved the limits on bodies/overlap to 32k and 65k
+	
+2006 June 25 Erwin Coumans
+	Added basic Collision Filtering, during broadphase
+	Allow adding meshes to the TriangleIndexVertexArray,
+	(input for TriangleMeshShape) 
+	Preparation for CompoundShape
+
+2006 June 19 Erwin Coumans
+	Added support for COLLADA Physics Import.
+	Both jam and Visual Studio can compile ColladaDemo
+
+2006 June 18 Dirk Gregorius <dirk@dirkgregorius.de>
+	Started implementing Generic6DOF joint and setup basic interface
+
+
+2006 June 17 Frank Richter <resqu@gmx.ch>
+	Bumped version in configure.ac to 1.5.6 (assuming that "1.5f" is
+	  the next version released).
+	Updated files in mk/autoconf and mk/jam with copies from CS; fixes a 
+	  GLU detection issue on MinGW.
+	Set msvc/bullet_ico.ico as the default application icon.
+	Disabled exceptions for gcc builds.
+	Applied a patch from Michael D. Adams to fix a warning with gcc.
+2006 jUNE 16 Erwin Coumans
+	Constraints now merge simulation islands.
+
+2006 May 24
+	Improved GJK accuracy, fixed GjkConvexCast issue, thanks to ~MyXa~ for reporting
+	
+2006 May 19
+	Added restitution support
+	Moved out Friction and Dynamics info from ManifoldPoint (removed logical dependency)
+	Added a void* m_userPersistentData in ManifoldPoint.
+	Added a ContactDestroyedCallback, to allow user to handle destruction of m_userPersistentData
+
+2006 May 13
+	Fixed some bugs in friction / jacobian calculations. Reported by Dirk Gregorius. Thanks!
+
+2006 May 9
+	Fixed raycasting filtering
+	Moved repository to SVN at https://svn.sourceforge.net/svnroot/bullet
+		
+2006 April 27
+	Moved raycasting to CollisionWorld, to make it more generic
+	Added basic CCD option in the CcdCollisionDemo
+	Fixed 'noResponse' mode, for triggering rigidbodies (useful for Artificial Intelligence queries)
+	Improved Bullet/ODE sample (in Extras)
+		
+2006 April 10
+	Separating Axis Test (SAT) convex hull collision detector, contribution by Simon Hobbs
+	Added SIMD SSE Math classes (for above SAT)
+	Added Mouse picking in CcdPhysicsDemo
+	Improved penetration depth estimation in MinkowskiPenetrationDepthSolver, both accuracy and performance
+	Added Hinge constraint
+	Added quickprof profiling (see http://sourceforge.net/projects/quickprof )
+		
+2006 March 21 Frank Richter <resqu@gmx.ch>
+	Removed VC manifest files.
+	Removed superfluous "grpplugins" projects.
+
+2006 March 20 Erwin Coumans
+	Clamped the acculumated impulse rather then intermediate impulse (within the iteration)
+	Use the persistent contacts for reusing the impulse
+	Separated friction and normal solving for better stability
+	Decreased the default number of iterations of the constraint solver from 10 to 4
+
+2006 March 19 Frank Richter <resqu@gmx.ch>
+	Removed a couple of CSisms from the VC projects.
+	Fixed VC include & lib paths to go to the Addtional* options
+	  instead the command line arguments.
+	Added pkgconfig support.
+
+2006 March 14 Frank Richter <resqu@gmx.ch>
+	Added support for shipped GLUT on MinGW.
+	Fixed GLUT support on MinGW.
+
+2006 March 13 Frank Richter <resqu@gmx.ch>
+	Bolted on Jam-based build system.
+	Generated VC project files.
+	Fixed GCC warnings.
+	Fixed Linux build issues.
+
+2006 March 13
+Added 3D Sweep and Prune Broadphase Collision Detection, Contribution from Simon Hobbs.
+
+2006 March 2
+	Minor change in license to ZLib/LibPNG
+	This makes it legally a bit easier to deploy on Playstation 3
+	Prepared for more generic constraints, added ConstraintsDemo
+
+2006 Feb 23
+	Rearranged files and dependencies to allow for easier standalone Collision Detection without Bullet Dynamics.
+	See Demos/CollisionInterfaceDemo and Extras/ode/ode/test/test_BulletGjk.cpp for examples how to use.
+
+2005 August 6
+	Bullet 0.2 release with demos, sources, doxygen, draft manual
+
+2005 June 1
+	First public release of Bullet
+	
+
+... todo: add history
+
+2003 Initial version (continuous collision detection)
Index: code/branches/physics/src/bullet/LICENSE
===================================================================
--- code/branches/physics/src/bullet/LICENSE	(revision 1963)
+++ code/branches/physics/src/bullet/LICENSE	(revision 1963)
@@ -0,0 +1,19 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+All files in the Bullet/src folder are under this Zlib license.
+Optional Extras/GIMPACT and Extras/GIMPACTBullet is also under ZLib license. Other optional external libraries in  Extras/Demos have own license,see respective files.
+
+This means Bullet can freely be used in any software, including commercial and console software. A Playstation 3 optimized version is available through Sony.
Index: code/branches/physics/src/bullet/LinearMath/CMakeLists.txt
===================================================================
--- code/branches/physics/src/bullet/LinearMath/CMakeLists.txt	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/CMakeLists.txt	(revision 1963)
@@ -0,0 +1,30 @@
+ADD_LIBRARY(LibBulletLinearMath
+		btAlignedObjectArray.h
+		btList.h
+		btPoolAllocator.h
+		btRandom.h
+		btVector3.h
+		btDefaultMotionState.h
+		btMatrix3x3.h
+		btQuadWord.h
+		btHashMap.h
+		btScalar.h
+		btAabbUtil2.h
+		btConvexHull.h
+		btConvexHull.cpp
+		btMinMax.h
+		btQuaternion.h
+		btStackAlloc.h
+		btGeometryUtil.h
+		btMotionState.h
+		btTransform.h
+		btAlignedAllocator.h
+		btIDebugDraw.h
+		btPoint3.h
+		btQuickprof.h
+		btTransformUtil.h
+		btQuickprof.cpp
+		btGeometryUtil.cpp
+		btAlignedAllocator.cpp
+)
+
Index: code/branches/physics/src/bullet/LinearMath/btAabbUtil2.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btAabbUtil2.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btAabbUtil2.h	(revision 1963)
@@ -0,0 +1,202 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef AABB_UTIL2
+#define AABB_UTIL2
+
+#include "btTransform.h"
+#include "btVector3.h"
+#include "btMinMax.h"
+
+SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
+								   btVector3& aabbMax,
+								   const btVector3& expansionMin,
+								   const btVector3& expansionMax)
+{
+	aabbMin = aabbMin + expansionMin;
+	aabbMax = aabbMax + expansionMax;
+}
+
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
+								const btVector3 &aabbMin2, const btVector3 &aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
+									const btVector3 &aabbMin, const btVector3 &aabbMax)
+{
+	const btVector3 &p1 = vertices[0];
+	const btVector3 &p2 = vertices[1];
+	const btVector3 &p3 = vertices[2];
+
+	if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+	if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+	if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+	if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+  
+	if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+	if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+	return true;
+}
+
+
+SIMD_FORCE_INLINE int	btOutcode(const btVector3& p,const btVector3& halfExtent) 
+{
+	return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
+		   (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
+		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
+		   (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
+		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
+		   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+
+SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
+								  const btVector3& rayInvDirection,
+								  const unsigned int raySign[3],
+								  const btVector3 bounds[2],
+								  btScalar& tmin,
+								  btScalar lambda_min,
+								  btScalar lambda_max)
+{
+	btScalar tmax, tymin, tymax, tzmin, tzmax;
+	tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
+	tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
+	tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
+	tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
+
+	if ( (tmin > tymax) || (tymin > tmax) )
+		return false;
+
+	if (tymin > tmin)
+		tmin = tymin;
+
+	if (tymax < tmax)
+		tmax = tymax;
+
+	tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
+	tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
+
+	if ( (tmin > tzmax) || (tzmin > tmax) )
+		return false;
+	if (tzmin > tmin)
+		tmin = tzmin;
+	if (tzmax < tmax)
+		tmax = tzmax;
+	return ( (tmin < lambda_max) && (tmax > lambda_min) );
+}
+
+SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
+								 const btVector3& rayTo, 
+								 const btVector3& aabbMin, 
+								 const btVector3& aabbMax,
+					  btScalar& param, btVector3& normal) 
+{
+	btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
+	btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
+	btVector3	source = rayFrom - aabbCenter;
+	btVector3	target = rayTo - aabbCenter;
+	int	sourceOutcode = btOutcode(source,aabbHalfExtent);
+	int targetOutcode = btOutcode(target,aabbHalfExtent);
+	if ((sourceOutcode & targetOutcode) == 0x0)
+	{
+		btScalar lambda_enter = btScalar(0.0);
+		btScalar lambda_exit  = param;
+		btVector3 r = target - source;
+		int i;
+		btScalar	normSign = 1;
+		btVector3	hitNormal(0,0,0);
+		int bit=1;
+
+		for (int j=0;j<2;j++)
+		{
+			for (i = 0; i != 3; ++i)
+			{
+				if (sourceOutcode & bit)
+				{
+					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					if (lambda_enter <= lambda)
+					{
+						lambda_enter = lambda;
+						hitNormal.setValue(0,0,0);
+						hitNormal[i] = normSign;
+					}
+				}
+				else if (targetOutcode & bit) 
+				{
+					btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					btSetMin(lambda_exit, lambda);
+				}
+				bit<<=1;
+			}
+			normSign = btScalar(-1.);
+		}
+		if (lambda_enter <= lambda_exit)
+		{
+			param = lambda_enter;
+			normal = hitNormal;
+			return true;
+		}
+	}
+	return false;
+}
+
+
+
+SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+	btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
+	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btVector3 center = t.getOrigin();
+	btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
+		   abs_b[1].dot(halfExtentsWithMargin),
+		  abs_b[2].dot(halfExtentsWithMargin));
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
+}
+
+
+SIMD_FORCE_INLINE	void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+		btAssert(localAabbMin.getX() <= localAabbMax.getX());
+		btAssert(localAabbMin.getY() <= localAabbMax.getY());
+		btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+		btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+		localHalfExtents+=btVector3(margin,margin,margin);
+
+		btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+		btMatrix3x3 abs_b = trans.getBasis().absolute();  
+		btVector3 center = trans(localCenter);
+		btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+			   abs_b[1].dot(localHalfExtents),
+			  abs_b[2].dot(localHalfExtents));
+		aabbMinOut = center-extent;
+		aabbMaxOut = center+extent;
+}
+
+
+#endif
+
+
Index: code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.cpp
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.cpp	(revision 1963)
@@ -0,0 +1,201 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btAlignedAllocator.h"
+
+int gNumAlignedAllocs = 0;
+int gNumAlignedFree = 0;
+int gTotalBytesAlignedAllocs = 0;//detect memory leaks
+
+#if defined (BT_HAS_ALIGNED_ALLOCATOR)
+#include <malloc.h>
+static void *btAlignedAllocDefault(size_t size, int alignment)
+{
+	return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void btAlignedFreeDefault(void *ptr)
+{
+	_aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void *btAlignedAllocDefault(size_t size, int alignment)
+{
+	return memalign(alignment, size);
+}
+
+static inline void btAlignedFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+#else
+static inline void *btAlignedAllocDefault(size_t size, int alignment)
+{
+  void *ret;
+  char *real;
+  unsigned long offset;
+
+  real = (char *)malloc(size + sizeof(void *) + (alignment-1));
+  if (real) {
+    offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
+    ret = (void *)((real + sizeof(void *)) + offset);
+    *((void **)(ret)-1) = (void *)(real);
+  } else {
+    ret = (void *)(real);
+  }
+  return (ret);
+}
+
+static inline void btAlignedFreeDefault(void *ptr)
+{
+  void* real;
+
+  if (ptr) {
+    real = *((void **)(ptr)-1);
+    free(real);
+  }
+}
+#endif
+
+static void *btAllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void btFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
+static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
+static btAllocFunc *sAllocFunc = btAllocDefault;
+static btFreeFunc *sFreeFunc = btFreeDefault;
+
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
+{
+  sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
+  sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
+}
+
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
+{
+  sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
+  sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
+}
+
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void*   btAlignedAllocInternal  (size_t size, int alignment,int line,char* filename)
+{
+ void *ret;
+ char *real;
+ unsigned long offset;
+
+ gTotalBytesAlignedAllocs += size;
+ gNumAlignedAllocs++;
+
+ 
+ real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
+ if (real) {
+   offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) &
+(alignment-1);
+   ret = (void *)((real + 2*sizeof(void *)) + offset);
+   *((void **)(ret)-1) = (void *)(real);
+       *((int*)(ret)-2) = size;
+
+ } else {
+   ret = (void *)(real);//??
+ }
+
+ printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size);
+
+ int* ptr = (int*)ret;
+ *ptr = 12;
+ return (ret);
+}
+
+void    btAlignedFreeInternal   (void* ptr,int line,char* filename)
+{
+
+ void* real;
+ gNumAlignedFree++;
+
+ if (ptr) {
+   real = *((void **)(ptr)-1);
+       int size = *((int*)(ptr)-2);
+       gTotalBytesAlignedAllocs -= size;
+
+	   printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size);
+
+   sFreeFunc(real);
+ } else
+ {
+	 printf("NULL ptr\n");
+ }
+}
+
+#else //BT_DEBUG_MEMORY_ALLOCATIONS
+
+void*	btAlignedAllocInternal	(size_t size, int alignment)
+{
+	gNumAlignedAllocs++;
+  void* ptr;
+#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+	ptr = sAlignedAllocFunc(size, alignment);
+#else
+  char *real;
+  unsigned long offset;
+
+  real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
+  if (real) {
+    offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
+    ptr = (void *)((real + sizeof(void *)) + offset);
+    *((void **)(ptr)-1) = (void *)(real);
+  } else {
+    ptr = (void *)(real);
+  }
+#endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+//	printf("btAlignedAllocInternal %d, %x\n",size,ptr);
+	return ptr;
+}
+
+void	btAlignedFreeInternal	(void* ptr)
+{
+	if (!ptr)
+	{
+		return;
+	}
+
+	gNumAlignedFree++;
+//	printf("btAlignedFreeInternal %x\n",ptr);
+#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+	sAlignedFreeFunc(ptr);
+#else
+  void* real;
+
+  if (ptr) {
+    real = *((void **)(ptr)-1);
+    sFreeFunc(real);
+  }
+#endif  // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__)
+}
+
+#endif //BT_DEBUG_MEMORY_ALLOCATIONS
+
Index: code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btAlignedAllocator.h	(revision 1963)
@@ -0,0 +1,104 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_ALIGNED_ALLOCATOR
+#define BT_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "btScalar.h"
+//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+
+#define btAlignedAlloc(a,b) \
+		btAlignedAllocInternal(a,b,__LINE__,__FILE__)
+
+#define btAlignedFree(ptr) \
+		btAlignedFreeInternal(ptr,__LINE__,__FILE__)
+
+void*	btAlignedAllocInternal	(size_t size, int alignment,int line,char* filename);
+
+void	btAlignedFreeInternal	(void* ptr,int line,char* filename);
+
+#else
+	void*	btAlignedAllocInternal	(size_t size, int alignment);
+	void	btAlignedFreeInternal	(void* ptr);
+
+	#define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
+	#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
+
+#endif
+typedef int	size_type;
+
+typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
+typedef void (btAlignedFreeFunc)(void *memblock);
+typedef void *(btAllocFunc)(size_t size);
+typedef void (btFreeFunc)(void *memblock);
+
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+
+///The btAlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
+template < typename T , unsigned Alignment >
+class btAlignedAllocator {
+	
+	typedef btAlignedAllocator< T , Alignment > self_type;
+	
+public:
+
+	//just going down a list:
+	btAlignedAllocator() {}
+	/*
+	btAlignedAllocator( const self_type & ) {}
+	*/
+
+	template < typename Other >
+	btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
+
+	typedef const T*         const_pointer;
+	typedef const T&         const_reference;
+	typedef T*               pointer;
+	typedef T&               reference;
+	typedef T                value_type;
+
+	pointer       address   ( reference        ref ) const                           { return &ref; }
+	const_pointer address   ( const_reference  ref ) const                           { return &ref; }
+	pointer       allocate  ( size_type        n   , const_pointer *      hint = 0 ) {
+		(void)hint;
+		return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
+	}
+	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
+	void          deallocate( pointer          ptr ) {
+		btAlignedFree( reinterpret_cast< void * >( ptr ) );
+	}
+	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
+	
+
+	template < typename O > struct rebind {
+		typedef btAlignedAllocator< O , Alignment > other;
+	};
+	template < typename O >
+	self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
+
+	friend bool operator==( const self_type & , const self_type & ) { return true; }
+};
+
+
+
+#endif //BT_ALIGNED_ALLOCATOR
+
Index: code/branches/physics/src/bullet/LinearMath/btAlignedObjectArray.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btAlignedObjectArray.h	(revision 1963)
@@ -0,0 +1,426 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_OBJECT_ARRAY__
+#define BT_OBJECT_ARRAY__
+
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btAlignedAllocator.h"
+
+///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
+///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define BT_USE_PLACEMENT_NEW 1
+//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+
+#ifdef BT_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif //BT_USE_MEMCPY
+
+#ifdef BT_USE_PLACEMENT_NEW
+#include <new> //for placement new
+#endif //BT_USE_PLACEMENT_NEW
+
+
+///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
+template <typename T> 
+//template <class T> 
+class btAlignedObjectArray
+{
+	btAlignedAllocator<T , 16>	m_allocator;
+
+	int					m_size;
+	int					m_capacity;
+	T*					m_data;
+	//PCK: added this line
+	bool				m_ownsMemory;
+
+	protected:
+		SIMD_FORCE_INLINE	int	allocSize(int size)
+		{
+			return (size ? size*2 : 1);
+		}
+		SIMD_FORCE_INLINE	void	copy(int start,int end, T* dest)
+		{
+			int i;
+			for (i=start;i<end;++i)
+#ifdef BT_USE_PLACEMENT_NEW
+				new (&dest[i]) T(m_data[i]);
+#else
+				dest[i] = m_data[i];
+#endif //BT_USE_PLACEMENT_NEW
+		}
+
+		SIMD_FORCE_INLINE	void	init()
+		{
+			//PCK: added this line
+			m_ownsMemory = true;
+			m_data = 0;
+			m_size = 0;
+			m_capacity = 0;
+		}
+		SIMD_FORCE_INLINE	void	destroy(int first,int last)
+		{
+			int i;
+			for (i=first; i<last;i++)
+			{
+				m_data[i].~T();
+			}
+		}
+
+		SIMD_FORCE_INLINE	void* allocate(int size)
+		{
+			if (size)
+				return m_allocator.allocate(size);
+			return 0;
+		}
+
+		SIMD_FORCE_INLINE	void	deallocate()
+		{
+			if(m_data)	{
+				//PCK: enclosed the deallocation in this block
+				if (m_ownsMemory)
+				{
+					m_allocator.deallocate(m_data);
+				}
+				m_data = 0;
+			}
+		}
+
+	
+
+
+	public:
+		
+		btAlignedObjectArray()
+		{
+			init();
+		}
+
+		~btAlignedObjectArray()
+		{
+			clear();
+		}
+
+		SIMD_FORCE_INLINE	int capacity() const
+		{	// return current length of allocated storage
+			return m_capacity;
+		}
+		
+		SIMD_FORCE_INLINE	int size() const
+		{	// return length of sequence
+			return m_size;
+		}
+		
+		SIMD_FORCE_INLINE const T& operator[](int n) const
+		{
+			return m_data[n];
+		}
+
+		SIMD_FORCE_INLINE T& operator[](int n)
+		{
+			return m_data[n];
+		}
+		
+
+		SIMD_FORCE_INLINE	void	clear()
+		{
+			destroy(0,size());
+			
+			deallocate();
+			
+			init();
+		}
+
+		SIMD_FORCE_INLINE	void	pop_back()
+		{
+			m_size--;
+			m_data[m_size].~T();
+		}
+
+		SIMD_FORCE_INLINE	void	resize(int newsize, const T& fillData=T())
+		{
+			int curSize = size();
+
+			if (newsize < size())
+			{
+				for(int i = curSize; i < newsize; i++)
+				{
+					m_data[i].~T();
+				}
+			} else
+			{
+				if (newsize > size())
+				{
+					reserve(newsize);
+				}
+#ifdef BT_USE_PLACEMENT_NEW
+				for (int i=curSize;i<newsize;i++)
+				{
+					new ( &m_data[i]) T(fillData);
+				}
+#endif //BT_USE_PLACEMENT_NEW
+
+			}
+
+			m_size = newsize;
+		}
+	
+
+		SIMD_FORCE_INLINE	T&  expand( const T& fillValue=T())
+		{	
+			int sz = size();
+			if( sz == capacity() )
+			{
+				reserve( allocSize(size()) );
+			}
+			m_size++;
+#ifdef BT_USE_PLACEMENT_NEW
+			new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+#endif
+
+			return m_data[sz];		
+		}
+
+
+		SIMD_FORCE_INLINE	void push_back(const T& _Val)
+		{	
+			int sz = size();
+			if( sz == capacity() )
+			{
+				reserve( allocSize(size()) );
+			}
+			
+#ifdef BT_USE_PLACEMENT_NEW
+			new ( &m_data[m_size] ) T(_Val);
+#else
+			m_data[size()] = _Val;			
+#endif //BT_USE_PLACEMENT_NEW
+
+			m_size++;
+		}
+
+	
+		
+		SIMD_FORCE_INLINE	void reserve(int _Count)
+		{	// determine new minimum length of allocated storage
+			if (capacity() < _Count)
+			{	// not enough room, reallocate
+				T*	s = (T*)allocate(_Count);
+
+				copy(0, size(), s);
+
+				destroy(0,size());
+
+				deallocate();
+				
+				//PCK: added this line
+				m_ownsMemory = true;
+
+				m_data = s;
+				
+				m_capacity = _Count;
+
+			}
+		}
+
+
+		class less
+		{
+			public:
+
+				bool operator() ( const T& a, const T& b )
+				{
+					return ( a < b );
+				}
+		};
+	
+		template <typename L>
+		void quickSortInternal(L CompareFunc,int lo, int hi)
+		{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+			int i=lo, j=hi;
+			T x=m_data[(lo+hi)/2];
+
+			//  partition
+			do
+			{    
+				while (CompareFunc(m_data[i],x)) 
+					i++; 
+				while (CompareFunc(x,m_data[j])) 
+					j--;
+				if (i<=j)
+				{
+					swap(i,j);
+					i++; j--;
+				}
+			} while (i<=j);
+
+			//  recursion
+			if (lo<j) 
+				quickSortInternal( CompareFunc, lo, j);
+			if (i<hi) 
+				quickSortInternal( CompareFunc, i, hi);
+		}
+
+
+		template <typename L>
+		void quickSort(L CompareFunc)
+		{
+			//don't sort 0 or 1 elements
+			if (size()>1)
+			{
+				quickSortInternal(CompareFunc,0,size()-1);
+			}
+		}
+
+
+		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+		template <typename L>
+		void downHeap(T *pArr, int k, int n,L CompareFunc)
+		{
+			/*  PRE: a[k+1..N] is a heap */
+			/* POST:  a[k..N]  is a heap */
+			
+			T temp = pArr[k - 1];
+			/* k has child(s) */
+			while (k <= n/2) 
+			{
+				int child = 2*k;
+				
+				if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
+				{
+					child++;
+				}
+				/* pick larger child */
+				if (CompareFunc(temp , pArr[child - 1]))
+				{
+					/* move child up */
+					pArr[k - 1] = pArr[child - 1];
+					k = child;
+				}
+				else
+				{
+					break;
+				}
+			}
+			pArr[k - 1] = temp;
+		} /*downHeap*/
+
+		void	swap(int index0,int index1)
+		{
+#ifdef BT_USE_MEMCPY
+			char	temp[sizeof(T)];
+			memcpy(temp,&m_data[index0],sizeof(T));
+			memcpy(&m_data[index0],&m_data[index1],sizeof(T));
+			memcpy(&m_data[index1],temp,sizeof(T));
+#else
+			T temp = m_data[index0];
+			m_data[index0] = m_data[index1];
+			m_data[index1] = temp;
+#endif //BT_USE_PLACEMENT_NEW
+
+		}
+
+	template <typename L>
+	void heapSort(L CompareFunc)
+	{
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = m_size;
+		for (k = n/2; k > 0; k--) 
+		{
+			downHeap(m_data, k, n, CompareFunc);
+		}
+
+		/* a[1..N] is now a heap */
+		while ( n>=1 ) 
+		{
+			swap(0,n-1); /* largest of a[0..n-1] */
+
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(m_data, 1, n, CompareFunc);
+		} 
+	}
+
+	///non-recursive binary search, assumes sorted array
+	int	findBinarySearch(const T& key) const
+	{
+		int first = 0;
+		int last = size();
+
+		//assume sorted array
+		while (first <= last) {
+			int mid = (first + last) / 2;  // compute mid point.
+			if (key > m_data[mid]) 
+				first = mid + 1;  // repeat search in top half.
+			else if (key < m_data[mid]) 
+				last = mid - 1; // repeat search in bottom half.
+			else
+				return mid;     // found it. return position /////
+		}
+		return size();    // failed to find key
+	}
+
+
+	int	findLinearSearch(const T& key) const
+	{
+		int index=size();
+		int i;
+
+		for (i=0;i<size();i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void	remove(const T& key)
+	{
+
+		int findIndex = findLinearSearch(key);
+		if (findIndex<size())
+		{
+			swap( findIndex,size()-1);
+			pop_back();
+		}
+	}
+
+	//PCK: whole function
+	void initializeFromBuffer(void *buffer, int size, int capacity)
+	{
+		clear();
+		m_ownsMemory = false;
+		m_data = (T*)buffer;
+		m_size = size;
+		m_capacity = capacity;
+	}
+
+};
+
+#endif //BT_OBJECT_ARRAY__
Index: code/branches/physics/src/bullet/LinearMath/btConvexHull.cpp
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btConvexHull.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btConvexHull.cpp	(revision 1963)
@@ -0,0 +1,1174 @@
+/*
+Stan Melax Convex Hull Computation
+Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <string.h>
+
+#include "btConvexHull.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btVector3.h"
+
+
+
+template <class T>
+void Swap(T &a,T &b)
+{
+	T tmp = a;
+	a=b;
+	b=tmp;
+}
+
+
+//----------------------------------
+
+class int3  
+{
+public:
+	int x,y,z;
+	int3(){};
+	int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
+	const int& operator[](int i) const {return (&x)[i];}
+	int& operator[](int i) {return (&x)[i];}
+};
+
+
+//------- btPlane ----------
+
+
+inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
+inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
+inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
+
+
+//--------- Utility Functions ------
+
+btVector3  PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
+btVector3  PlaneProject(const btPlane &plane, const btVector3 &point);
+
+btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
+btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
+{
+	btVector3 N1 = p0.normal;
+	btVector3 N2 = p1.normal;
+	btVector3 N3 = p2.normal;
+
+	btVector3 n2n3; n2n3 = N2.cross(N3);
+	btVector3 n3n1; n3n1 = N3.cross(N1);
+	btVector3 n1n2; n1n2 = N1.cross(N2);
+
+	btScalar quotient = (N1.dot(n2n3));
+
+	btAssert(btFabs(quotient) > btScalar(0.000001));
+	
+	quotient = btScalar(-1.) / quotient;
+	n2n3 *= p0.dist;
+	n3n1 *= p1.dist;
+	n1n2 *= p2.dist;
+	btVector3 potentialVertex = n2n3;
+	potentialVertex += n3n1;
+	potentialVertex += n1n2;
+	potentialVertex *= quotient;
+
+	btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
+	return result;
+
+}
+
+btScalar   DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
+btVector3  TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
+btVector3  NormalOf(const btVector3 *vert, const int n);
+
+
+btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
+{
+	// returns the point where the line p0-p1 intersects the plane n&d
+				static btVector3 dif;
+		dif = p1-p0;
+				btScalar dn= dot(plane.normal,dif);
+				btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn;
+				return p0 + (dif*t);
+}
+
+btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
+{
+	return point - plane.normal * (dot(point,plane.normal)+plane.dist);
+}
+
+btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
+{
+	// return the normal of the triangle
+	// inscribed by v0, v1, and v2
+	btVector3 cp=cross(v1-v0,v2-v1);
+	btScalar m=cp.length();
+	if(m==0) return btVector3(1,0,0);
+	return cp*(btScalar(1.0)/m);
+}
+
+
+btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
+{
+	static btVector3 cp;
+	cp = cross(udir,vdir).normalized();
+
+	btScalar distu = -dot(cp,ustart);
+	btScalar distv = -dot(cp,vstart);
+	btScalar dist = (btScalar)fabs(distu-distv);
+	if(upoint) 
+		{
+		btPlane plane;
+		plane.normal = cross(vdir,cp).normalized();
+		plane.dist = -dot(plane.normal,vstart);
+		*upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
+	}
+	if(vpoint) 
+		{
+		btPlane plane;
+		plane.normal = cross(udir,cp).normalized();
+		plane.dist = -dot(plane.normal,ustart);
+		*vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
+	}
+	return dist;
+}
+
+
+
+
+
+
+
+#define COPLANAR   (0)
+#define UNDER      (1)
+#define OVER       (2)
+#define SPLIT      (OVER|UNDER)
+#define PAPERWIDTH (btScalar(0.001))
+
+btScalar planetestepsilon = PAPERWIDTH;
+
+
+
+typedef ConvexH::HalfEdge HalfEdge;
+
+ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
+{
+	vertices.resize(vertices_size);
+	edges.resize(edges_size);
+	facets.resize(facets_size);
+}
+
+
+int PlaneTest(const btPlane &p, const btVector3 &v);
+int PlaneTest(const btPlane &p, const btVector3 &v) {
+	btScalar a  = dot(v,p.normal)+p.dist;
+	int   flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
+	return flag;
+}
+
+int SplitTest(ConvexH &convex,const btPlane &plane);
+int SplitTest(ConvexH &convex,const btPlane &plane) {
+	int flag=0;
+	for(int i=0;i<convex.vertices.size();i++) {
+		flag |= PlaneTest(plane,convex.vertices[i]);
+	}
+	return flag;
+}
+
+class VertFlag
+{
+public:
+	unsigned char planetest;
+	unsigned char junk;
+	unsigned char undermap;
+	unsigned char overmap;
+};
+class EdgeFlag 
+{
+public:
+	unsigned char planetest;
+	unsigned char fixes;
+	short undermap;
+	short overmap;
+};
+class PlaneFlag
+{
+public:
+	unsigned char undermap;
+	unsigned char overmap;
+};
+class Coplanar{
+public:
+	unsigned short ea;
+	unsigned char v0;
+	unsigned char v1;
+};
+
+
+
+
+
+
+
+
+template<class T>
+int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+{
+	btAssert(count);
+	int m=-1;
+	for(int i=0;i<count;i++) 
+		if(allow[i])
+		{
+			if(m==-1 || dot(p[i],dir)>dot(p[m],dir))
+				m=i;
+		}
+	btAssert(m!=-1);
+	return m;
+} 
+
+btVector3 orth(const btVector3 &v);
+btVector3 orth(const btVector3 &v)
+{
+	btVector3 a=cross(v,btVector3(0,0,1));
+	btVector3 b=cross(v,btVector3(0,1,0));
+	if (a.length() > b.length())
+	{
+		return a.normalized();
+	} else {
+		return b.normalized();
+	}
+}
+
+
+template<class T>
+int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
+{
+	int m=-1;
+	while(m==-1)
+	{
+		m = maxdirfiltered(p,count,dir,allow);
+		if(allow[m]==3) return m;
+		T u = orth(dir);
+		T v = cross(u,dir);
+		int ma=-1;
+		for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
+		{
+			btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
+			btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
+			int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
+			if(ma==m && mb==m)
+			{
+				allow[m]=3;
+				return m;
+			}
+			if(ma!=-1 && ma!=mb)  // Yuck - this is really ugly
+			{
+				int mc = ma;
+				for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
+				{
+					btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
+					btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
+					int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
+					if(mc==m && md==m)
+					{
+						allow[m]=3;
+						return m;
+					}
+					mc=md;
+				}
+			}
+			ma=mb;
+		}
+		allow[m]=0;
+		m=-1;
+	}
+	btAssert(0);
+	return m;
+} 
+
+
+
+
+int operator ==(const int3 &a,const int3 &b);
+int operator ==(const int3 &a,const int3 &b) 
+{
+	for(int i=0;i<3;i++) 
+	{
+		if(a[i]!=b[i]) return 0;
+	}
+	return 1;
+}
+
+
+int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
+int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) 
+{
+	btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
+	return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
+}
+int hasedge(const int3 &t, int a,int b);
+int hasedge(const int3 &t, int a,int b)
+{
+	for(int i=0;i<3;i++)
+	{
+		int i1= (i+1)%3;
+		if(t[i]==a && t[i1]==b) return 1;
+	}
+	return 0;
+}
+int hasvert(const int3 &t, int v);
+int hasvert(const int3 &t, int v)
+{
+	return (t[0]==v || t[1]==v || t[2]==v) ;
+}
+int shareedge(const int3 &a,const int3 &b);
+int shareedge(const int3 &a,const int3 &b)
+{
+	int i;
+	for(i=0;i<3;i++)
+	{
+		int i1= (i+1)%3;
+		if(hasedge(a,b[i1],b[i])) return 1;
+	}
+	return 0;
+}
+
+class Tri;
+
+
+
+class Tri : public int3
+{
+public:
+	int3 n;
+	int id;
+	int vmax;
+	btScalar rise;
+	Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
+	{
+		vmax=-1;
+		rise = btScalar(0.0);
+	}
+	~Tri()
+	{
+	}
+	int &neib(int a,int b);
+};
+
+
+int &Tri::neib(int a,int b)
+{
+	static int er=-1;
+	int i;
+	for(i=0;i<3;i++) 
+	{
+		int i1=(i+1)%3;
+		int i2=(i+2)%3;
+		if((*this)[i]==a && (*this)[i1]==b) return n[i2];
+		if((*this)[i]==b && (*this)[i1]==a) return n[i2];
+	}
+	btAssert(0);
+	return er;
+}
+void HullLibrary::b2bfix(Tri* s,Tri*t)
+{
+	int i;
+	for(i=0;i<3;i++) 
+	{
+		int i1=(i+1)%3;
+		int i2=(i+2)%3;
+		int a = (*s)[i1];
+		int b = (*s)[i2];
+		btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
+		btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
+		m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
+		m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
+	}
+}
+
+void HullLibrary::removeb2b(Tri* s,Tri*t)
+{
+	b2bfix(s,t);
+	deAllocateTriangle(s);
+
+	deAllocateTriangle(t);
+}
+
+void HullLibrary::checkit(Tri *t)
+{
+	(void)t;
+
+	int i;
+	btAssert(m_tris[t->id]==t);
+	for(i=0;i<3;i++)
+	{
+		int i1=(i+1)%3;
+		int i2=(i+2)%3;
+		int a = (*t)[i1];
+		int b = (*t)[i2];
+
+		// release compile fix
+		(void)i1;
+		(void)i2;
+		(void)a;
+		(void)b;
+
+		btAssert(a!=b);
+		btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
+	}
+}
+
+Tri*	HullLibrary::allocateTriangle(int a,int b,int c)
+{
+	void* mem = btAlignedAlloc(sizeof(Tri),16);
+	Tri* tr = new (mem)Tri(a,b,c);
+	tr->id = m_tris.size();
+	m_tris.push_back(tr);
+
+	return tr;
+}
+
+void	HullLibrary::deAllocateTriangle(Tri* tri)
+{
+	btAssert(m_tris[tri->id]==tri);
+	m_tris[tri->id]=NULL;
+	tri->~Tri();
+	btAlignedFree(tri);
+}
+
+
+void HullLibrary::extrude(Tri *t0,int v)
+{
+	int3 t= *t0;
+	int n = m_tris.size();
+	Tri* ta = allocateTriangle(v,t[1],t[2]);
+	ta->n = int3(t0->n[0],n+1,n+2);
+	m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
+	Tri* tb = allocateTriangle(v,t[2],t[0]);
+	tb->n = int3(t0->n[1],n+2,n+0);
+	m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
+	Tri* tc = allocateTriangle(v,t[0],t[1]);
+	tc->n = int3(t0->n[2],n+0,n+1);
+	m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
+	checkit(ta);
+	checkit(tb);
+	checkit(tc);
+	if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
+	if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
+	if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
+	deAllocateTriangle(t0);
+
+}
+
+Tri* HullLibrary::extrudable(btScalar epsilon)
+{
+	int i;
+	Tri *t=NULL;
+	for(i=0;i<m_tris.size();i++)
+	{
+		if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
+		{
+			t = m_tris[i];
+		}
+	}
+	return (t->rise >epsilon)?t:NULL ;
+}
+
+
+
+
+int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
+{
+	btVector3 basis[3];
+	basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );      
+	int p0 = maxdirsterid(verts,verts_count, basis[0],allow);   
+	int	p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
+	basis[0] = verts[p0]-verts[p1];
+	if(p0==p1 || basis[0]==btVector3(0,0,0)) 
+		return int4(-1,-1,-1,-1);
+	basis[1] = cross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
+	basis[2] = cross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
+	if (basis[1].length() > basis[2].length())
+	{
+		basis[1].normalize();
+	} else {
+		basis[1] = basis[2];
+		basis[1].normalize ();
+	}
+	int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
+	if(p2 == p0 || p2 == p1)
+	{
+		p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
+	}
+	if(p2 == p0 || p2 == p1) 
+		return int4(-1,-1,-1,-1);
+	basis[1] = verts[p2] - verts[p0];
+	basis[2] = cross(basis[1],basis[0]).normalized();
+	int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
+	if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
+	if(p3==p0||p3==p1||p3==p2) 
+		return int4(-1,-1,-1,-1);
+	btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
+	if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
+	return int4(p0,p1,p2,p3);
+}
+
+int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
+{
+	if(verts_count <4) return 0;
+	if(vlimit==0) vlimit=1000000000;
+	int j;
+	btVector3 bmin(*verts),bmax(*verts);
+	btAlignedObjectArray<int> isextreme;
+	isextreme.reserve(verts_count);
+	btAlignedObjectArray<int> allow;
+	allow.reserve(verts_count);
+
+	for(j=0;j<verts_count;j++) 
+	{
+		allow.push_back(1);
+		isextreme.push_back(0);
+		bmin.setMin (verts[j]);
+		bmax.setMax (verts[j]);
+	}
+	btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
+	btAssert (epsilon != 0.0);
+
+
+	int4 p = FindSimplex(verts,verts_count,allow);
+	if(p.x==-1) return 0; // simplex failed
+
+
+
+	btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);  // a valid interior point
+	Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
+	Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
+	Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
+	Tri *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
+	isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
+	checkit(t0);checkit(t1);checkit(t2);checkit(t3);
+
+	for(j=0;j<m_tris.size();j++)
+	{
+		Tri *t=m_tris[j];
+		btAssert(t);
+		btAssert(t->vmax<0);
+		btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
+		t->vmax = maxdirsterid(verts,verts_count,n,allow);
+		t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
+	}
+	Tri *te;
+	vlimit-=4;
+	while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
+	{
+		int3 ti=*te;
+		int v=te->vmax;
+		btAssert(v != -1);
+		btAssert(!isextreme[v]);  // wtf we've already done this vertex
+		isextreme[v]=1;
+		//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
+		j=m_tris.size();
+		while(j--) {
+			if(!m_tris[j]) continue;
+			int3 t=*m_tris[j];
+			if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) 
+			{
+				extrude(m_tris[j],v);
+			}
+		}
+		// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
+		j=m_tris.size();
+		while(j--)
+		{
+			if(!m_tris[j]) continue;
+			if(!hasvert(*m_tris[j],v)) break;
+			int3 nt=*m_tris[j];
+			if(above(verts,nt,center,btScalar(0.01)*epsilon)  || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
+			{
+				Tri *nb = m_tris[m_tris[j]->n[0]];
+				btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
+				extrude(nb,v);
+				j=m_tris.size(); 
+			}
+		} 
+		j=m_tris.size();
+		while(j--)
+		{
+			Tri *t=m_tris[j];
+			if(!t) continue;
+			if(t->vmax>=0) break;
+			btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
+			t->vmax = maxdirsterid(verts,verts_count,n,allow);
+			if(isextreme[t->vmax]) 
+			{
+				t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
+			}
+			else
+			{
+				t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]);
+			}
+		}
+		vlimit --;
+	}
+	return 1;
+}
+
+int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) 
+{
+	int rc=calchullgen(verts,verts_count,  vlimit) ;
+	if(!rc) return 0;
+	btAlignedObjectArray<int> ts;
+	int i;
+
+	for(i=0;i<m_tris.size();i++)
+	{
+		if(m_tris[i])
+		{
+			for(int j=0;j<3;j++)
+				ts.push_back((*m_tris[i])[j]);
+			deAllocateTriangle(m_tris[i]);
+		}
+	}
+	tris_count = ts.size()/3;
+	tris_out.resize(ts.size());
+	
+	for (i=0;i<ts.size();i++)
+	{
+		tris_out[i] = static_cast<unsigned int>(ts[i]);
+	}
+	m_tris.resize(0);
+
+	return 1;
+}
+
+
+
+
+
+bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
+{
+	
+	int    tris_count;
+	int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
+	if(!ret) return false;
+	result.mIndexCount = (unsigned int) (tris_count*3);
+	result.mFaceCount  = (unsigned int) tris_count;
+	result.mVertices   = (btVector3*) vertices;
+	result.mVcount     = (unsigned int) vcount;
+	return true;
+
+}
+
+
+void ReleaseHull(PHullResult &result);
+void ReleaseHull(PHullResult &result)
+{
+	if ( result.m_Indices.size() )
+	{
+		result.m_Indices.clear();
+	}
+
+	result.mVcount = 0;
+	result.mIndexCount = 0;
+	result.mVertices = 0;
+}
+
+
+//*********************************************************************
+//*********************************************************************
+//********  HullLib header
+//*********************************************************************
+//*********************************************************************
+
+//*********************************************************************
+//*********************************************************************
+//********  HullLib implementation
+//*********************************************************************
+//*********************************************************************
+
+HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
+																					HullResult           &result)         // contains the resulst
+{
+	HullError ret = QE_FAIL;
+
+
+	PHullResult hr;
+
+	unsigned int vcount = desc.mVcount;
+	if ( vcount < 8 ) vcount = 8;
+
+	btAlignedObjectArray<btVector3> vertexSource;
+	vertexSource.resize(static_cast<int>(vcount));
+
+	btVector3 scale;
+
+	unsigned int ovcount;
+
+	bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
+
+	if ( ok )
+	{
+
+
+//		if ( 1 ) // scale vertices back to their original size.
+		{
+			for (unsigned int i=0; i<ovcount; i++)
+			{
+				btVector3& v = vertexSource[static_cast<int>(i)];
+				v[0]*=scale[0];
+				v[1]*=scale[1];
+				v[2]*=scale[2];
+			}
+		}
+
+		ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
+
+		if ( ok )
+		{
+
+			// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
+			btAlignedObjectArray<btVector3>	vertexScratch;
+			vertexScratch.resize(static_cast<int>(hr.mVcount));
+
+			BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
+
+			ret = QE_OK;
+
+			if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
+			{
+				result.mPolygons          = false;
+				result.mNumOutputVertices = ovcount;
+				result.m_OutputVertices.resize(static_cast<int>(ovcount));
+				result.mNumFaces          = hr.mFaceCount;
+				result.mNumIndices        = hr.mIndexCount;
+
+				result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
+
+				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+
+  			if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+				{
+
+					const unsigned int *source = &hr.m_Indices[0];
+					unsigned int *dest   = &result.m_Indices[0];
+
+					for (unsigned int i=0; i<hr.mFaceCount; i++)
+					{
+						dest[0] = source[2];
+						dest[1] = source[1];
+						dest[2] = source[0];
+						dest+=3;
+						source+=3;
+					}
+
+				}
+				else
+				{
+					memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
+				}
+			}
+			else
+			{
+				result.mPolygons          = true;
+				result.mNumOutputVertices = ovcount;
+				result.m_OutputVertices.resize(static_cast<int>(ovcount));
+				result.mNumFaces          = hr.mFaceCount;
+				result.mNumIndices        = hr.mIndexCount+hr.mFaceCount;
+				result.m_Indices.resize(static_cast<int>(result.mNumIndices));
+				memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
+
+//				if ( 1 )
+				{
+					const unsigned int *source = &hr.m_Indices[0];
+					unsigned int *dest   = &result.m_Indices[0];
+					for (unsigned int i=0; i<hr.mFaceCount; i++)
+					{
+						dest[0] = 3;
+						if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
+						{
+							dest[1] = source[2];
+							dest[2] = source[1];
+							dest[3] = source[0];
+						}
+						else
+						{
+							dest[1] = source[0];
+							dest[2] = source[1];
+							dest[3] = source[2];
+						}
+
+						dest+=4;
+						source+=3;
+					}
+				}
+			}
+			ReleaseHull(hr);
+		}
+	}
+
+	return ret;
+}
+
+
+
+HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
+{
+	if ( result.m_OutputVertices.size())
+	{
+		result.mNumOutputVertices=0;
+		result.m_OutputVertices.clear();
+	}
+	if ( result.m_Indices.size() )
+	{
+		result.mNumIndices=0;
+		result.m_Indices.clear();
+	}
+	return QE_OK;
+}
+
+
+static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
+{
+	// XXX, might be broken
+	btVector3& dest = p[vcount];
+	dest[0] = x;
+	dest[1] = y;
+	dest[2] = z;
+	vcount++;
+}
+
+btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
+btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
+{
+
+	btScalar dx = px - p2[0];
+	btScalar dy = py - p2[1];
+	btScalar dz = pz - p2[2];
+
+	return dx*dx+dy*dy+dz*dz;
+}
+
+
+
+bool  HullLibrary::CleanupVertices(unsigned int svcount,
+				   const btVector3 *svertices,
+				   unsigned int stride,
+				   unsigned int &vcount,       // output number of vertices
+				   btVector3 *vertices,                 // location to store the results.
+				   btScalar  normalepsilon,
+				   btVector3& scale)
+{
+	if ( svcount == 0 ) return false;
+
+	m_vertexIndexMapping.resize(0);
+
+
+#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
+
+	vcount = 0;
+
+	btScalar recip[3];
+
+	if ( scale )
+	{
+		scale[0] = 1;
+		scale[1] = 1;
+		scale[2] = 1;
+	}
+
+	btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
+	btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+
+	const char *vtx = (const char *) svertices;
+
+//	if ( 1 )
+	{
+		for (unsigned int i=0; i<svcount; i++)
+		{
+			const btScalar *p = (const btScalar *) vtx;
+
+			vtx+=stride;
+
+			for (int j=0; j<3; j++)
+			{
+				if ( p[j] < bmin[j] ) bmin[j] = p[j];
+				if ( p[j] > bmax[j] ) bmax[j] = p[j];
+			}
+		}
+	}
+
+	btScalar dx = bmax[0] - bmin[0];
+	btScalar dy = bmax[1] - bmin[1];
+	btScalar dz = bmax[2] - bmin[2];
+
+	btVector3 center;
+
+	center[0] = dx*btScalar(0.5) + bmin[0];
+	center[1] = dy*btScalar(0.5) + bmin[1];
+	center[2] = dz*btScalar(0.5) + bmin[2];
+
+	if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
+	{
+
+		btScalar len = FLT_MAX;
+
+		if ( dx > EPSILON && dx < len ) len = dx;
+		if ( dy > EPSILON && dy < len ) len = dy;
+		if ( dz > EPSILON && dz < len ) len = dz;
+
+		if ( len == FLT_MAX )
+		{
+			dx = dy = dz = btScalar(0.01); // one centimeter
+		}
+		else
+		{
+			if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
+			if ( dy < EPSILON ) dy = len * btScalar(0.05);
+			if ( dz < EPSILON ) dz = len * btScalar(0.05);
+		}
+
+		btScalar x1 = center[0] - dx;
+		btScalar x2 = center[0] + dx;
+
+		btScalar y1 = center[1] - dy;
+		btScalar y2 = center[1] + dy;
+
+		btScalar z1 = center[2] - dz;
+		btScalar z2 = center[2] + dz;
+
+		addPoint(vcount,vertices,x1,y1,z1);
+		addPoint(vcount,vertices,x2,y1,z1);
+		addPoint(vcount,vertices,x2,y2,z1);
+		addPoint(vcount,vertices,x1,y2,z1);
+		addPoint(vcount,vertices,x1,y1,z2);
+		addPoint(vcount,vertices,x2,y1,z2);
+		addPoint(vcount,vertices,x2,y2,z2);
+		addPoint(vcount,vertices,x1,y2,z2);
+
+		return true; // return cube
+
+
+	}
+	else
+	{
+		if ( scale )
+		{
+			scale[0] = dx;
+			scale[1] = dy;
+			scale[2] = dz;
+
+			recip[0] = 1 / dx;
+			recip[1] = 1 / dy;
+			recip[2] = 1 / dz;
+
+			center[0]*=recip[0];
+			center[1]*=recip[1];
+			center[2]*=recip[2];
+
+		}
+
+	}
+
+
+
+	vtx = (const char *) svertices;
+
+	for (unsigned int i=0; i<svcount; i++)
+	{
+		const btVector3 *p = (const btVector3 *)vtx;
+		vtx+=stride;
+
+		btScalar px = p->getX();
+		btScalar py = p->getY();
+		btScalar pz = p->getZ();
+
+		if ( scale )
+		{
+			px = px*recip[0]; // normalize
+			py = py*recip[1]; // normalize
+			pz = pz*recip[2]; // normalize
+		}
+
+//		if ( 1 )
+		{
+			unsigned int j;
+
+			for (j=0; j<vcount; j++)
+			{
+				/// XXX might be broken
+				btVector3& v = vertices[j];
+
+				btScalar x = v[0];
+				btScalar y = v[1];
+				btScalar z = v[2];
+
+				btScalar dx = fabsf(x - px );
+				btScalar dy = fabsf(y - py );
+				btScalar dz = fabsf(z - pz );
+
+				if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
+				{
+					// ok, it is close enough to the old one
+					// now let us see if it is further from the center of the point cloud than the one we already recorded.
+					// in which case we keep this one instead.
+
+					btScalar dist1 = GetDist(px,py,pz,center);
+					btScalar dist2 = GetDist(v[0],v[1],v[2],center);
+
+					if ( dist1 > dist2 )
+					{
+						v[0] = px;
+						v[1] = py;
+						v[2] = pz;
+						
+					}
+
+					break;
+				}
+			}
+
+			if ( j == vcount )
+			{
+				btVector3& dest = vertices[vcount];
+				dest[0] = px;
+				dest[1] = py;
+				dest[2] = pz;
+				vcount++;
+			}
+			m_vertexIndexMapping.push_back(j);
+		}
+	}
+
+	// ok..now make sure we didn't prune so many vertices it is now invalid.
+//	if ( 1 )
+	{
+		btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
+		btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
+
+		for (unsigned int i=0; i<vcount; i++)
+		{
+			const btVector3& p = vertices[i];
+			for (int j=0; j<3; j++)
+			{
+				if ( p[j] < bmin[j] ) bmin[j] = p[j];
+				if ( p[j] > bmax[j] ) bmax[j] = p[j];
+			}
+		}
+
+		btScalar dx = bmax[0] - bmin[0];
+		btScalar dy = bmax[1] - bmin[1];
+		btScalar dz = bmax[2] - bmin[2];
+
+		if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
+		{
+			btScalar cx = dx*btScalar(0.5) + bmin[0];
+			btScalar cy = dy*btScalar(0.5) + bmin[1];
+			btScalar cz = dz*btScalar(0.5) + bmin[2];
+
+			btScalar len = FLT_MAX;
+
+			if ( dx >= EPSILON && dx < len ) len = dx;
+			if ( dy >= EPSILON && dy < len ) len = dy;
+			if ( dz >= EPSILON && dz < len ) len = dz;
+
+			if ( len == FLT_MAX )
+			{
+				dx = dy = dz = btScalar(0.01); // one centimeter
+			}
+			else
+			{
+				if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
+				if ( dy < EPSILON ) dy = len * btScalar(0.05);
+				if ( dz < EPSILON ) dz = len * btScalar(0.05);
+			}
+
+			btScalar x1 = cx - dx;
+			btScalar x2 = cx + dx;
+
+			btScalar y1 = cy - dy;
+			btScalar y2 = cy + dy;
+
+			btScalar z1 = cz - dz;
+			btScalar z2 = cz + dz;
+
+			vcount = 0; // add box
+
+			addPoint(vcount,vertices,x1,y1,z1);
+			addPoint(vcount,vertices,x2,y1,z1);
+			addPoint(vcount,vertices,x2,y2,z1);
+			addPoint(vcount,vertices,x1,y2,z1);
+			addPoint(vcount,vertices,x1,y1,z2);
+			addPoint(vcount,vertices,x2,y1,z2);
+			addPoint(vcount,vertices,x2,y2,z2);
+			addPoint(vcount,vertices,x1,y2,z2);
+
+			return true;
+		}
+	}
+
+	return true;
+}
+
+void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
+{
+	btAlignedObjectArray<int>tmpIndices;
+	tmpIndices.resize(m_vertexIndexMapping.size());
+	int i;
+
+	for (i=0;i<m_vertexIndexMapping.size();i++)
+	{
+		tmpIndices[i] = m_vertexIndexMapping[i];
+	}
+
+	TUIntArray usedIndices;
+	usedIndices.resize(static_cast<int>(vcount));
+	memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
+
+	ocount = 0;
+
+	for (i=0; i<indexcount; i++)
+	{
+		unsigned int v = indices[i]; // original array index
+
+		btAssert( v >= 0 && v < vcount );
+
+		if ( usedIndices[static_cast<int>(v)] ) // if already remapped
+		{
+			indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
+		}
+		else
+		{
+
+			indices[i] = ocount;      // new index mapping
+
+			overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
+			overts[ocount][1] = verts[v][1];
+			overts[ocount][2] = verts[v][2];
+
+			for (int k=0;k<m_vertexIndexMapping.size();k++)
+			{
+				if (tmpIndices[k]==v)
+					m_vertexIndexMapping[k]=ocount;
+			}
+
+			ocount++; // increment output vert count
+
+			btAssert( ocount >=0 && ocount <= vcount );
+
+			usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
+
+		
+		}
+	}
+
+	
+}
Index: code/branches/physics/src/bullet/LinearMath/btConvexHull.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btConvexHull.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btConvexHull.h	(revision 1963)
@@ -0,0 +1,245 @@
+
+/*
+Stan Melax Convex Hull Computation
+Copyright (c) 2008 Stan Melax http://www.melax.com/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///includes modifications/improvements by John Ratcliff, see BringOutYourDead below.
+
+#ifndef CD_HULL_H
+#define CD_HULL_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+typedef btAlignedObjectArray<unsigned int> TUIntArray;
+
+class HullResult
+{
+public:
+	HullResult(void)
+	{
+		mPolygons = true;
+		mNumOutputVertices = 0;
+		mNumFaces = 0;
+		mNumIndices = 0;
+	}
+	bool                    mPolygons;                  // true if indices represents polygons, false indices are triangles
+	unsigned int            mNumOutputVertices;         // number of vertices in the output hull
+	btAlignedObjectArray<btVector3>	m_OutputVertices;            // array of vertices
+	unsigned int            mNumFaces;                  // the number of faces produced
+	unsigned int            mNumIndices;                // the total number of indices
+	btAlignedObjectArray<unsigned int>    m_Indices;                   // pointer to indices.
+
+// If triangles, then indices are array indexes into the vertex list.
+// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
+};
+
+enum HullFlag
+{
+	QF_TRIANGLES         = (1<<0),             // report results as triangles, not polygons.
+	QF_REVERSE_ORDER     = (1<<1),             // reverse order of the triangle indices.
+	QF_DEFAULT           = QF_TRIANGLES
+};
+
+
+class HullDesc
+{
+public:
+	HullDesc(void)
+	{
+		mFlags          = QF_DEFAULT;
+		mVcount         = 0;
+		mVertices       = 0;
+		mVertexStride   = sizeof(btVector3);
+		mNormalEpsilon  = 0.001f;
+		mMaxVertices	= 4096; // maximum number of points to be considered for a convex hull.
+		mMaxFaces	= 4096;
+	};
+
+	HullDesc(HullFlag flag,
+		 unsigned int vcount,
+		 const btVector3 *vertices,
+		 unsigned int stride = sizeof(btVector3))
+	{
+		mFlags          = flag;
+		mVcount         = vcount;
+		mVertices       = vertices;
+		mVertexStride   = stride;
+		mNormalEpsilon  = btScalar(0.001);
+		mMaxVertices    = 4096;
+	}
+
+	bool HasHullFlag(HullFlag flag) const
+	{
+		if ( mFlags & flag ) return true;
+		return false;
+	}
+
+	void SetHullFlag(HullFlag flag)
+	{
+		mFlags|=flag;
+	}
+
+	void ClearHullFlag(HullFlag flag)
+	{
+		mFlags&=~flag;
+	}
+
+	unsigned int      mFlags;           // flags to use when generating the convex hull.
+	unsigned int      mVcount;          // number of vertices in the input point cloud
+	const btVector3  *mVertices;        // the array of vertices.
+	unsigned int      mVertexStride;    // the stride of each vertex, in bytes.
+	btScalar             mNormalEpsilon;   // the epsilon for removing duplicates.  This is a normalized value, if normalized bit is on.
+	unsigned int      mMaxVertices;     // maximum number of vertices to be considered for the hull!
+	unsigned int      mMaxFaces;
+};
+
+enum HullError
+{
+	QE_OK,            // success!
+	QE_FAIL           // failed.
+};
+
+class btPlane
+{
+	public:
+	btVector3	normal;
+	btScalar	dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+			btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
+			btPlane():normal(),dist(0){}
+	
+};
+
+
+
+class ConvexH 
+{
+  public:
+	class HalfEdge
+	{
+	  public:
+		short ea;         // the other half of the edge (index into edges list)
+		unsigned char v;  // the vertex at the start of this edge (index into vertices list)
+		unsigned char p;  // the facet on which this edge lies (index into facets list)
+		HalfEdge(){}
+		HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
+	};
+	ConvexH()
+	{
+		int i;
+		i=0;
+	}
+	~ConvexH()
+	{
+		int i;
+		i=0;
+	}
+	btAlignedObjectArray<btVector3> vertices;
+	btAlignedObjectArray<HalfEdge> edges;
+	btAlignedObjectArray<btPlane>  facets;
+	ConvexH(int vertices_size,int edges_size,int facets_size);
+};
+
+
+class int4
+{
+public:
+	int x,y,z,w;
+	int4(){};
+	int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
+	const int& operator[](int i) const {return (&x)[i];}
+	int& operator[](int i) {return (&x)[i];}
+};
+
+class PHullResult
+{
+public:
+
+	PHullResult(void)
+	{
+		mVcount = 0;
+		mIndexCount = 0;
+		mFaceCount = 0;
+		mVertices = 0;
+	}
+
+	unsigned int mVcount;
+	unsigned int mIndexCount;
+	unsigned int mFaceCount;
+	btVector3*   mVertices;
+	TUIntArray m_Indices;
+};
+
+
+
+///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method.
+///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape.
+class HullLibrary
+{
+
+	btAlignedObjectArray<class Tri*> m_tris;
+
+public:
+
+	btAlignedObjectArray<int> m_vertexIndexMapping;
+
+
+	HullError CreateConvexHull(const HullDesc& desc, // describes the input request
+				   HullResult&     result);        // contains the resulst
+	HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
+
+private:
+
+	bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
+
+	class Tri*	allocateTriangle(int a,int b,int c);
+	void	deAllocateTriangle(Tri*);
+	void b2bfix(Tri* s,Tri*t);
+
+	void removeb2b(Tri* s,Tri*t);
+
+	void checkit(Tri *t);
+
+	Tri* extrudable(btScalar epsilon);
+
+	int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
+
+	int calchullgen(btVector3 *verts,int verts_count, int vlimit);
+
+	int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
+
+	class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
+
+	void extrude(class Tri* t0,int v);
+
+	ConvexH* test_cube();
+
+	//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. 
+	//After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
+	//The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
+	//The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
+	void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
+
+	bool CleanupVertices(unsigned int svcount,
+			     const btVector3* svertices,
+			     unsigned int stride,
+			     unsigned int &vcount, // output number of vertices
+			     btVector3* vertices, // location to store the results.
+			     btScalar  normalepsilon,
+			     btVector3& scale);
+};
+
+
+#endif
+
Index: code/branches/physics/src/bullet/LinearMath/btDefaultMotionState.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btDefaultMotionState.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btDefaultMotionState.h	(revision 1963)
@@ -0,0 +1,38 @@
+#ifndef DEFAULT_MOTION_STATE_H
+#define DEFAULT_MOTION_STATE_H
+
+///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
+struct	btDefaultMotionState : public btMotionState
+{
+	btTransform m_graphicsWorldTrans;
+	btTransform	m_centerOfMassOffset;
+	btTransform m_startWorldTrans;
+	void*		m_userPointer;
+
+	btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
+		: m_graphicsWorldTrans(startTrans),
+		m_centerOfMassOffset(centerOfMassOffset),
+		m_startWorldTrans(startTrans),
+		m_userPointer(0)
+
+	{
+	}
+
+	///synchronizes world transform from user to physics
+	virtual void	getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
+	{
+			centerOfMassWorldTrans = 	m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
+	}
+
+	///synchronizes world transform from physics to user
+	///Bullet only calls the update of worldtransform for active objects
+	virtual void	setWorldTransform(const btTransform& centerOfMassWorldTrans)
+	{
+			m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
+	}
+
+	
+
+};
+
+#endif //DEFAULT_MOTION_STATE_H
Index: code/branches/physics/src/bullet/LinearMath/btGeometryUtil.cpp
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btGeometryUtil.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btGeometryUtil.cpp	(revision 1963)
@@ -0,0 +1,185 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btGeometryUtil.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C"
+{	
+	void btBulletMathProbe ();
+
+	void btBulletMathProbe () {}
+}
+
+
+bool	btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
+		if (dist>btScalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+		
+}
+
+
+bool	btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin)
+{
+	int numvertices = vertices.size();
+	for (int i=0;i<numvertices;i++)
+	{
+		const btVector3& N1 = vertices[i];
+		btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
+		if (dist>btScalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
+
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		if (planeEquation.dot(N1) > btScalar(0.999))
+		{
+			return false;
+		} 
+	}
+	return true;
+}
+
+void	btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
+{
+		const int numvertices = vertices.size();
+	// brute force:
+	for (int i=0;i<numvertices;i++)
+	{
+		const btVector3& N1 = vertices[i];
+		
+
+		for (int j=i+1;j<numvertices;j++)
+		{
+			const btVector3& N2 = vertices[j];
+				
+			for (int k=j+1;k<numvertices;k++)
+			{
+
+				const btVector3& N3 = vertices[k];
+
+				btVector3 planeEquation,edge0,edge1;
+				edge0 = N2-N1;
+				edge1 = N3-N1;
+				btScalar normalSign = btScalar(1.);
+				for (int ww=0;ww<2;ww++)
+				{
+					planeEquation = normalSign * edge0.cross(edge1);
+					if (planeEquation.length2() > btScalar(0.0001))
+					{
+						planeEquation.normalize();
+						if (notExist(planeEquation,planeEquationsOut))
+						{
+							planeEquation[3] = -planeEquation.dot(N1);
+							
+								//check if inside, and replace supportingVertexOut if needed
+								if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
+								{
+									planeEquationsOut.push_back(planeEquation);
+								}
+						}
+					}
+					normalSign = btScalar(-1.);
+				}
+			
+			}
+		}
+	}
+
+}
+
+void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
+{
+	const int numbrushes = planeEquations.size();
+	// brute force:
+	for (int i=0;i<numbrushes;i++)
+	{
+		const btVector3& N1 = planeEquations[i];
+		
+
+		for (int j=i+1;j<numbrushes;j++)
+		{
+			const btVector3& N2 = planeEquations[j];
+				
+			for (int k=j+1;k<numbrushes;k++)
+			{
+
+				const btVector3& N3 = planeEquations[k];
+
+				btVector3 n2n3; n2n3 = N2.cross(N3);
+				btVector3 n3n1; n3n1 = N3.cross(N1);
+				btVector3 n1n2; n1n2 = N1.cross(N2);
+				
+				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
+					 ( n3n1.length2() > btScalar(0.0001) ) &&
+					 ( n1n2.length2() > btScalar(0.0001) ) )
+				{
+					//point P out of 3 plane equations:
+
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
+					//P =  -------------------------------------------------------------------------  
+					//   N1 . ( N2 * N3 )  
+
+
+					btScalar quotient = (N1.dot(n2n3));
+					if (btFabs(quotient) > btScalar(0.000001))
+					{
+						quotient = btScalar(-1.) / quotient;
+						n2n3 *= N1[3];
+						n3n1 *= N2[3];
+						n1n2 *= N3[3];
+						btVector3 potentialVertex = n2n3;
+						potentialVertex += n3n1;
+						potentialVertex += n1n2;
+						potentialVertex *= quotient;
+
+						//check if inside, and replace supportingVertexOut if needed
+						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
+						{
+							verticesOut.push_back(potentialVertex);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
Index: code/branches/physics/src/bullet/LinearMath/btGeometryUtil.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btGeometryUtil.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btGeometryUtil.h	(revision 1963)
@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_GEOMETRY_UTIL_H
+#define BT_GEOMETRY_UTIL_H
+
+#include "btVector3.h"
+#include "btAlignedObjectArray.h"
+
+///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
+class btGeometryUtil
+{
+	public:
+	
+	
+		static void	getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
+
+		static void	getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
+	
+		static bool	isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar	margin);
+		
+		static bool	isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin);
+
+		static bool	areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin);
+
+};
+
+
+#endif //BT_GEOMETRY_UTIL_H
+
Index: code/branches/physics/src/bullet/LinearMath/btHashMap.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btHashMap.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btHashMap.h	(revision 1963)
@@ -0,0 +1,303 @@
+#ifndef BT_HASH_MAP_H
+#define BT_HASH_MAP_H
+
+#include "btAlignedObjectArray.h"
+
+const int BT_HASH_NULL=0xffffffff;
+
+template <class Value>
+class btHashKey
+{
+	int	m_uid;
+public:
+
+	btHashKey(int uid)
+		:m_uid(uid)
+	{
+	}
+
+	int	getUid() const
+	{
+		return m_uid;
+	}
+
+	//to our success
+	SIMD_FORCE_INLINE	unsigned int getHash()const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return key;
+	}
+
+	btHashKey	getKey(const Value& value) const
+	{
+		return btHashKey(value.getUid());
+	}
+};
+
+
+template <class Value>
+class btHashKeyPtr
+{
+	int	m_uid;
+public:
+
+	btHashKeyPtr(int uid)
+		:m_uid(uid)
+	{
+	}
+
+	int	getUid() const
+	{
+		return m_uid;
+	}
+
+	//to our success
+	SIMD_FORCE_INLINE	unsigned int getHash()const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return key;
+	}
+
+	btHashKeyPtr	getKey(const Value& value) const
+	{
+		return btHashKeyPtr(value->getUid());
+	}
+};
+
+///The btHashMap template class implements a generic and lightweight hashmap.
+///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
+template <class Key, class Value>
+class btHashMap
+{
+
+	btAlignedObjectArray<int>		m_hashTable;
+	btAlignedObjectArray<int>		m_next;
+	btAlignedObjectArray<Value>		m_valueArray;
+
+
+
+	void	growTables(const Key& key)
+	{
+		int newCapacity = m_valueArray.capacity();
+
+		if (m_hashTable.size() < newCapacity)
+		{
+			//grow hashtable and next table
+			int curHashtableSize = m_hashTable.size();
+
+			m_hashTable.resize(newCapacity);
+			m_next.resize(newCapacity);
+
+			int i;
+
+			for (i= 0; i < newCapacity; ++i)
+			{
+				m_hashTable[i] = BT_HASH_NULL;
+			}
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_next[i] = BT_HASH_NULL;
+			}
+
+			for(i=0;i<curHashtableSize;i++)
+			{
+				const Value& value = m_valueArray[i];
+
+				int	hashValue = key.getKey(value).getHash() & (m_valueArray.capacity()-1);	// New hash value with new mask
+				m_next[i] = m_hashTable[hashValue];
+				m_hashTable[hashValue] = i;
+			}
+
+
+		}
+	}
+
+	public:
+
+	void insert(const Key& key, const Value& value) {
+		int hash = key.getHash() & (m_valueArray.capacity()-1);
+		//don't add it if it is already there
+		if (find(key))
+		{
+			return;
+		}
+		int count = m_valueArray.size();
+		int oldCapacity = m_valueArray.capacity();
+		m_valueArray.push_back(value);
+		int newCapacity = m_valueArray.capacity();
+		if (oldCapacity < newCapacity)
+		{
+			growTables(key);
+			//hash with new capacity
+			hash = key.getHash() & (m_valueArray.capacity()-1);
+		}
+		m_next[count] = m_hashTable[hash];
+		m_hashTable[hash] = count;
+	}
+
+	void remove(const Key& key) {
+
+		int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+		int pairIndex = findIndex(key);
+		
+		if (pairIndex ==BT_HASH_NULL)
+		{
+			return;
+		}
+
+		// Remove the pair from the hash table.
+		int index = m_hashTable[hash];
+		btAssert(index != BT_HASH_NULL);
+
+		int previous = BT_HASH_NULL;
+		while (index != pairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != BT_HASH_NULL)
+		{
+			btAssert(m_next[previous] == pairIndex);
+			m_next[previous] = m_next[pairIndex];
+		}
+		else
+		{
+			m_hashTable[hash] = m_next[pairIndex];
+		}
+
+		// We now move the last pair into spot of the
+		// pair being removed. We need to fix the hash
+		// table indices to support the move.
+
+		int lastPairIndex = m_valueArray.size() - 1;
+
+		// If the removed pair is the last pair, we are done.
+		if (lastPairIndex == pairIndex)
+		{
+			m_valueArray.pop_back();
+			return;
+		}
+
+		// Remove the last pair from the hash table.
+		const Value* lastValue = &m_valueArray[lastPairIndex];
+		int lastHash = key.getKey(*lastValue).getHash() & (m_valueArray.capacity()-1);
+
+		index = m_hashTable[lastHash];
+		btAssert(index != BT_HASH_NULL);
+
+		previous = BT_HASH_NULL;
+		while (index != lastPairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != BT_HASH_NULL)
+		{
+			btAssert(m_next[previous] == lastPairIndex);
+			m_next[previous] = m_next[lastPairIndex];
+		}
+		else
+		{
+			m_hashTable[lastHash] = m_next[lastPairIndex];
+		}
+
+		// Copy the last pair into the remove pair's spot.
+		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+
+		// Insert the last pair into the hash table
+		m_next[pairIndex] = m_hashTable[lastHash];
+		m_hashTable[lastHash] = pairIndex;
+
+		m_valueArray.pop_back();
+
+	}
+
+
+	int size() const
+	{
+		return m_valueArray.size();
+	}
+
+	const Value* getAtIndex(int index) const
+	{
+		btAssert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Value* getAtIndex(int index)
+	{
+		btAssert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Value* operator[](const Key& key) {
+		return find(key);
+	}
+
+	const Value*	find(const Key& key) const
+	{
+		int index = findIndex(key);
+		if (index == BT_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	Value*	find(const Key& key)
+	{
+		int index = findIndex(key);
+		if (index == BT_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+
+	int	findIndex(const Key& key) const
+	{
+		int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+		if (hash >= m_hashTable.size())
+		{
+			return BT_HASH_NULL;
+		}
+
+		int index = m_hashTable[hash];
+		while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false)
+		{
+			index = m_next[index];
+		}
+		return index;
+	}
+
+	void	clear()
+	{
+		m_hashTable.clear();
+		m_next.clear();
+		m_valueArray.clear();
+	}
+
+};
+
+#endif //BT_HASH_MAP_H
Index: code/branches/physics/src/bullet/LinearMath/btIDebugDraw.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btIDebugDraw.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btIDebugDraw.h	(revision 1963)
@@ -0,0 +1,116 @@
+/*
+Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+
+#ifndef IDEBUG_DRAW__H
+#define IDEBUG_DRAW__H
+
+#include "btVector3.h"
+
+
+///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
+///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
+///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
+class	btIDebugDraw
+{
+	public:
+
+	enum	DebugDrawModes
+	{
+		DBG_NoDebug=0,
+		DBG_DrawWireframe = 1,
+		DBG_DrawAabb=2,
+		DBG_DrawFeaturesText=4,
+		DBG_DrawContactPoints=8,
+		DBG_NoDeactivation=16,
+		DBG_NoHelpText = 32,
+		DBG_DrawText=64,
+		DBG_ProfileTimings = 128,
+		DBG_EnableSatComparison = 256,
+		DBG_DisableBulletLCP = 512,
+		DBG_EnableCCD = 1024,
+		DBG_MAX_DEBUG_DRAW_MODE
+	};
+
+	virtual ~btIDebugDraw() {};
+
+	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
+	
+	virtual	void	drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
+	{
+		drawTriangle(v0,v1,v2,color,alpha);
+	}
+	virtual	void	drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/)
+	{
+		drawLine(v0,v1,color);
+		drawLine(v1,v2,color);
+		drawLine(v2,v0,color);
+	}
+
+	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
+
+	virtual void	reportErrorWarning(const char* warningString) = 0;
+
+	virtual void	draw3dText(const btVector3& location,const char* textString) = 0;
+	
+	virtual void	setDebugMode(int debugMode) =0;
+	
+	virtual int		getDebugMode() const = 0;
+
+	inline void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
+	{
+
+		btVector3 halfExtents = (to-from)* 0.5f;
+		btVector3 center = (to+from) *0.5f;
+		int i,j;
+
+		btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
+		for (i=0;i<4;i++)
+		{
+			for (j=0;j<3;j++)
+			{
+				pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],		
+					edgecoord[2]*halfExtents[2]);
+				pa+=center;
+
+				int othercoord = j%3;
+				edgecoord[othercoord]*=-1.f;
+				pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],	
+					edgecoord[2]*halfExtents[2]);
+				pb+=center;
+
+				drawLine(pa,pb,color);
+			}
+			edgecoord = btVector3(-1.f,-1.f,-1.f);
+			if (i<3)
+				edgecoord[i]*=-1.f;
+		}
+	}
+};
+
+
+#endif //IDEBUG_DRAW__H
+
Index: code/branches/physics/src/bullet/LinearMath/btList.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btList.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btList.h	(revision 1963)
@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_LIST_H
+#define GEN_LIST_H
+
+class btGEN_Link {
+public:
+    btGEN_Link() : m_next(0), m_prev(0) {}
+    btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
+    
+    btGEN_Link *getNext() const { return m_next; }  
+    btGEN_Link *getPrev() const { return m_prev; }  
+
+    bool isHead() const { return m_prev == 0; }
+    bool isTail() const { return m_next == 0; }
+
+    void insertBefore(btGEN_Link *link) {
+        m_next         = link;
+        m_prev         = link->m_prev;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void insertAfter(btGEN_Link *link) {
+        m_next         = link->m_next;
+        m_prev         = link;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void remove() { 
+        m_next->m_prev = m_prev; 
+        m_prev->m_next = m_next;
+    }
+
+private:  
+    btGEN_Link  *m_next;
+    btGEN_Link  *m_prev;
+};
+
+class btGEN_List {
+public:
+    btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+
+    btGEN_Link *getHead() const { return m_head.getNext(); } 
+    btGEN_Link *getTail() const { return m_tail.getPrev(); } 
+
+    void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
+    void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
+    
+private:
+    btGEN_Link m_head;
+    btGEN_Link m_tail;
+};
+
+#endif
+
+
+
Index: code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h	(revision 1963)
@@ -0,0 +1,504 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef btMatrix3x3_H
+#define btMatrix3x3_H
+
+#include "btScalar.h"
+
+#include "btVector3.h"
+#include "btQuaternion.h"
+
+
+
+///The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
+///Make sure to only include a pure orthogonal matrix without scaling.
+class btMatrix3x3 {
+	public:
+		btMatrix3x3 () {}
+		
+//		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+		
+		explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+		/*
+		template <typename btScalar>
+		Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+		{ 
+			setEulerYPR(yaw, pitch, roll);
+		}
+		*/
+		btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+				  const btScalar& yx, const btScalar& yy, const btScalar& yz,
+				  const btScalar& zx, const btScalar& zy, const btScalar& zz)
+		{ 
+			setValue(xx, xy, xz, 
+					 yx, yy, yz, 
+					 zx, zy, zz);
+		}
+		
+		SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+		{
+			m_el[0] = other.m_el[0];
+			m_el[1] = other.m_el[1];
+			m_el[2] = other.m_el[2];
+		}
+
+		SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+		{
+			m_el[0] = other.m_el[0];
+			m_el[1] = other.m_el[1];
+			m_el[2] = other.m_el[2];
+			return *this;
+		}
+
+		SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+		{
+			return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+		}
+		
+
+
+		SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+		{
+			return m_el[i];
+		}
+
+
+		SIMD_FORCE_INLINE btVector3&  operator[](int i)
+		{ 
+			btFullAssert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+		{
+			btFullAssert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		btMatrix3x3& operator*=(const btMatrix3x3& m); 
+		
+	
+	void setFromOpenGLSubMatrix(const btScalar *m)
+		{
+			m_el[0].setValue(m[0],m[4],m[8]);
+			m_el[1].setValue(m[1],m[5],m[9]);
+			m_el[2].setValue(m[2],m[6],m[10]);
+
+		}
+
+		void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
+					  const btScalar& yx, const btScalar& yy, const btScalar& yz, 
+					  const btScalar& zx, const btScalar& zy, const btScalar& zz)
+		{
+			m_el[0].setValue(xx,xy,xz);
+			m_el[1].setValue(yx,yy,yz);
+			m_el[2].setValue(zx,zy,zz);
+		}
+  
+		void setRotation(const btQuaternion& q) 
+		{
+			btScalar d = q.length2();
+			btFullAssert(d != btScalar(0.0));
+			btScalar s = btScalar(2.0) / d;
+			btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
+			btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
+			btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
+			btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+			setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+					 xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
+					 xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+		}
+		
+
+
+		void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
+		{
+
+			btScalar cy(btCos(yaw)); 
+			btScalar  sy(btSin(yaw)); 
+			btScalar  cp(btCos(pitch)); 
+			btScalar  sp(btSin(pitch)); 
+			btScalar  cr(btCos(roll));
+			btScalar  sr(btSin(roll));
+			btScalar  cc = cy * cr; 
+			btScalar  cs = cy * sr; 
+			btScalar  sc = sy * cr; 
+			btScalar  ss = sy * sr;
+			setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
+                     cp * sr,       cp * cr,      -sp,
+					 sc + sp * cs, -ss + sp * cc,  cy * cp);
+		
+		}
+
+	/**
+	 * setEulerZYX
+	 * @param euler a const reference to a btVector3 of euler angles
+	 * These angles are used to produce a rotation matrix. The euler
+	 * angles are applied in ZYX order. I.e a vector is first rotated 
+	 * about X then Y and then Z
+	 **/
+	
+	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
+		btScalar ci ( btCos(eulerX)); 
+		btScalar cj ( btCos(eulerY)); 
+		btScalar ch ( btCos(eulerZ)); 
+		btScalar si ( btSin(eulerX)); 
+		btScalar sj ( btSin(eulerY)); 
+		btScalar sh ( btSin(eulerZ)); 
+		btScalar cc = ci * ch; 
+		btScalar cs = ci * sh; 
+		btScalar sc = si * ch; 
+		btScalar ss = si * sh;
+		
+		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+				 cj * sh, sj * ss + cc, sj * cs - sc, 
+	       			 -sj,      cj * si,      cj * ci);
+	}
+
+		void setIdentity()
+		{ 
+			setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(1.0), btScalar(0.0), 
+					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
+		}
+    
+		void getOpenGLSubMatrix(btScalar *m) const 
+		{
+			m[0]  = btScalar(m_el[0].x()); 
+			m[1]  = btScalar(m_el[1].x());
+			m[2]  = btScalar(m_el[2].x());
+			m[3]  = btScalar(0.0); 
+			m[4]  = btScalar(m_el[0].y());
+			m[5]  = btScalar(m_el[1].y());
+			m[6]  = btScalar(m_el[2].y());
+			m[7]  = btScalar(0.0); 
+			m[8]  = btScalar(m_el[0].z()); 
+			m[9]  = btScalar(m_el[1].z());
+			m[10] = btScalar(m_el[2].z());
+			m[11] = btScalar(0.0); 
+		}
+
+		void getRotation(btQuaternion& q) const
+		{
+			btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+			btScalar temp[4];
+			
+			if (trace > btScalar(0.0)) 
+			{
+				btScalar s = btSqrt(trace + btScalar(1.0));
+				temp[3]=(s * btScalar(0.5));
+				s = btScalar(0.5) / s;
+				
+				temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+				temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+				temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+			} 
+			else 
+			{
+				int i = m_el[0].x() < m_el[1].y() ? 
+					(m_el[1].y() < m_el[2].z() ? 2 : 1) :
+					(m_el[0].x() < m_el[2].z() ? 2 : 0); 
+				int j = (i + 1) % 3;  
+				int k = (i + 2) % 3;
+				
+				btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
+				temp[i] = s * btScalar(0.5);
+				s = btScalar(0.5) / s;
+				
+				temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+				temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+				temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+			}
+			q.setValue(temp[0],temp[1],temp[2],temp[3]);
+		}
+	
+		void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+		{
+			
+			if (btScalar(m_el[1].z()) < btScalar(1))
+			{
+				if (btScalar(m_el[1].z()) > -btScalar(1))
+				{
+					yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+					pitch = btScalar(btAsin(-m_el[1].y()));
+					roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
+				}
+				else 
+				{
+					yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
+					pitch = SIMD_HALF_PI;
+					roll = btScalar(0.0);
+				}
+			}
+			else
+			{
+				yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
+				pitch = -SIMD_HALF_PI;
+				roll = btScalar(0.0);
+			}
+		}
+		
+
+	
+		
+		btMatrix3x3 scaled(const btVector3& s) const
+		{
+			return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+									 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+									 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+		}
+
+		btScalar            determinant() const;
+		btMatrix3x3 adjoint() const;
+		btMatrix3x3 absolute() const;
+		btMatrix3x3 transpose() const;
+		btMatrix3x3 inverse() const; 
+		
+		btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
+		btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
+		
+		SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
+		{
+			return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+		}
+		SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
+		{
+			return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+		}
+		SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
+		{
+			return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+		}
+		
+
+		///diagonalizes this matrix by the Jacobi method. rot stores the rotation
+		///from the coordinate system in which the matrix is diagonal to the original
+		///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
+		///stops when all off-diagonal elements are less than the threshold multiplied
+		///by the sum of the absolute values of the diagonal, or when maxSteps have
+		///been executed. Note that this matrix is assumed to be symmetric.
+		void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+		{
+		 rot.setIdentity();
+		 for (int step = maxSteps; step > 0; step--)
+		 {
+			// find off-diagonal element [p][q] with largest magnitude
+			int p = 0;
+			int q = 1;
+			int r = 2;
+			btScalar max = btFabs(m_el[0][1]);
+			btScalar v = btFabs(m_el[0][2]);
+			if (v > max)
+			{
+			   q = 2;
+			   r = 1;
+			   max = v;
+			}
+			v = btFabs(m_el[1][2]);
+			if (v > max)
+			{
+			   p = 1;
+			   q = 2;
+			   r = 0;
+			   max = v;
+			}
+
+			btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
+			if (max <= t)
+			{
+			   if (max <= SIMD_EPSILON * t)
+			   {
+				  return;
+			   }
+			   step = 1;
+			}
+
+			// compute Jacobi rotation J which leads to a zero for element [p][q] 
+			btScalar mpq = m_el[p][q];
+			btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+			btScalar theta2 = theta * theta;
+			btScalar cos;
+			btScalar sin;
+			if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
+			{
+			   t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+										: 1 / (theta - btSqrt(1 + theta2));
+			   cos = 1 / btSqrt(1 + t * t);
+			   sin = cos * t;
+			}
+			else
+			{
+			   // approximation for large theta-value, i.e., a nearly diagonal matrix
+			   t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+			   cos = 1 - btScalar(0.5) * t * t;
+			   sin = cos * t;
+			}
+
+			// apply rotation to matrix (this = J^T * this * J)
+			m_el[p][q] = m_el[q][p] = 0;
+			m_el[p][p] -= t * mpq;
+			m_el[q][q] += t * mpq;
+			btScalar mrp = m_el[r][p];
+			btScalar mrq = m_el[r][q];
+			m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+			m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+			// apply rotation to rot (rot = rot * J)
+			for (int i = 0; i < 3; i++)
+			{
+			   btVector3& row = rot[i];
+			   mrp = row[p];
+			   mrq = row[q];
+			   row[p] = cos * mrp - sin * mrq;
+			   row[q] = cos * mrq + sin * mrp;
+			}
+		 }
+		}
+
+
+		
+	protected:
+		btScalar cofac(int r1, int c1, int r2, int c2) const 
+		{
+			return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+		}
+
+		btVector3 m_el[3];
+	};
+	
+	SIMD_FORCE_INLINE btMatrix3x3& 
+	btMatrix3x3::operator*=(const btMatrix3x3& m)
+	{
+		setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+				 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+				 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+		return *this;
+	}
+	
+	SIMD_FORCE_INLINE btScalar 
+	btMatrix3x3::determinant() const
+	{ 
+		return triple((*this)[0], (*this)[1], (*this)[2]);
+	}
+	
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::absolute() const
+	{
+		return btMatrix3x3(
+			btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+			btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+			btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::transpose() const 
+	{
+		return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+								 m_el[0].y(), m_el[1].y(), m_el[2].y(),
+								 m_el[0].z(), m_el[1].z(), m_el[2].z());
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::adjoint() const 
+	{
+		return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+								 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+								 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::inverse() const
+	{
+		btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+		btScalar det = (*this)[0].dot(co);
+		btFullAssert(det != btScalar(0.0));
+		btScalar s = btScalar(1.0) / det;
+		return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+								 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+								 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+	{
+		return btMatrix3x3(
+			m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+			m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+			m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+			m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+			m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+			m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+			m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+			m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+			m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+	}
+	
+	SIMD_FORCE_INLINE btMatrix3x3 
+	btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+	{
+		return btMatrix3x3(
+			m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+			m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+			m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+		
+	}
+
+	SIMD_FORCE_INLINE btVector3 
+	operator*(const btMatrix3x3& m, const btVector3& v) 
+	{
+		return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+	}
+	
+
+	SIMD_FORCE_INLINE btVector3
+	operator*(const btVector3& v, const btMatrix3x3& m)
+	{
+		return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3 
+	operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+	{
+		return btMatrix3x3(
+			m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+			m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+			m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+	}
+
+/*
+	SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+    return btMatrix3x3(
+        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+
+SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+   return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+            m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+            m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+
+#endif
Index: code/branches/physics/src/bullet/LinearMath/btMinMax.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btMinMax.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btMinMax.h	(revision 1963)
@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_MINMAX_H
+#define GEN_MINMAX_H
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) 
+{
+  return a < b ? a : b ;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) 
+{
+  return  a > b ? a : b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) 
+{
+	return a < lb ? lb : (ub < a ? ub : a); 
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
+#endif
Index: code/branches/physics/src/bullet/LinearMath/btMotionState.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btMotionState.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btMotionState.h	(revision 1963)
@@ -0,0 +1,40 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MOTIONSTATE_H
+#define BT_MOTIONSTATE_H
+
+#include "btTransform.h"
+
+///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
+///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
+class	btMotionState
+{
+	public:
+		
+		virtual ~btMotionState()
+		{
+			
+		}
+		
+		virtual void	getWorldTransform(btTransform& worldTrans ) const =0;
+
+		//Bullet only calls the update of worldtransform for active objects
+		virtual void	setWorldTransform(const btTransform& worldTrans)=0;
+		
+	
+};
+
+#endif //BT_MOTIONSTATE_H
Index: code/branches/physics/src/bullet/LinearMath/btPoint3.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btPoint3.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btPoint3.h	(revision 1963)
@@ -0,0 +1,24 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef btPoint3_H
+#define btPoint3_H
+
+#include "btVector3.h"
+
+typedef btVector3 btPoint3;
+
+#endif
Index: code/branches/physics/src/bullet/LinearMath/btPoolAllocator.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btPoolAllocator.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btPoolAllocator.h	(revision 1963)
@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef _BT_POOL_ALLOCATOR_H
+#define _BT_POOL_ALLOCATOR_H
+
+#include "btScalar.h"
+#include "btAlignedAllocator.h"
+
+///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
+class btPoolAllocator
+{
+	int				m_elemSize;
+	int				m_maxElements;
+	int				m_freeCount;
+	void*			m_firstFree;
+	unsigned char*	m_pool;
+
+public:
+
+	btPoolAllocator(int elemSize, int maxElements)
+		:m_elemSize(elemSize),
+		m_maxElements(maxElements)
+	{
+		m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
+
+		unsigned char* p = m_pool;
+        m_firstFree = p;
+        m_freeCount = m_maxElements;
+        int count = m_maxElements;
+        while (--count) {
+            *(void**)p = (p + m_elemSize);
+            p += m_elemSize;
+        }
+        *(void**)p = 0;
+    }
+
+	~btPoolAllocator()
+	{
+		btAlignedFree( m_pool);
+	}
+
+	int	getFreeCount() const
+	{
+		return m_freeCount;
+	}
+
+	void*	allocate(int size)
+	{
+		// release mode fix
+		(void)size;
+		btAssert(!size || size<=m_elemSize);
+		btAssert(m_freeCount>0);
+        void* result = m_firstFree;
+        m_firstFree = *(void**)m_firstFree;
+        --m_freeCount;
+        return result;
+	}
+
+	bool validPtr(void* ptr)
+	{
+		if (ptr) {
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	void	freeMemory(void* ptr)
+	{
+		 if (ptr) {
+            btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+            *(void**)ptr = m_firstFree;
+            m_firstFree = ptr;
+            ++m_freeCount;
+        }
+	}
+
+	int	getElementSize() const
+	{
+		return m_elemSize;
+	}
+
+
+};
+
+#endif //_BT_POOL_ALLOCATOR_H
Index: code/branches/physics/src/bullet/LinearMath/btQuadWord.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btQuadWord.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btQuadWord.h	(revision 1963)
@@ -0,0 +1,142 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_QUADWORD_H
+#define SIMD_QUADWORD_H
+
+#include "btScalar.h"
+#include "btMinMax.h"
+#include <math.h>
+
+
+
+///The btQuadWordStorage class is base class for btVector3 and btQuaternion. 
+///Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. todo: look into this
+///ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
+class btQuadWordStorage
+{
+protected:
+
+	btScalar	m_x;
+	btScalar	m_y;
+	btScalar	m_z;
+	btScalar	m_unusedW;
+
+public:
+
+};
+
+
+///btQuadWord is base-class for vectors, points
+class	btQuadWord : public btQuadWordStorage
+{
+	public:
+	
+//		SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_x)[i];	}      
+//		SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; }
+
+		SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; }
+
+		SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; }
+
+		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; }
+
+		SIMD_FORCE_INLINE void	setX(btScalar x) { m_x = x;};
+
+		SIMD_FORCE_INLINE void	setY(btScalar y) { m_y = y;};
+
+		SIMD_FORCE_INLINE void	setZ(btScalar z) { m_z = z;};
+
+		SIMD_FORCE_INLINE void	setW(btScalar w) { m_unusedW = w;};
+
+		SIMD_FORCE_INLINE const btScalar& x() const { return m_x; }
+
+		SIMD_FORCE_INLINE const btScalar& y() const { return m_y; }
+
+		SIMD_FORCE_INLINE const btScalar& z() const { return m_z; }
+
+		SIMD_FORCE_INLINE const btScalar& w() const { return m_unusedW; }
+
+
+		SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_x; }
+		SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_x; }
+
+
+
+		SIMD_FORCE_INLINE void 	setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+			m_unusedW = 0.f;
+		}
+
+/*		void getValue(btScalar *m) const 
+		{
+			m[0] = m_x;
+			m[1] = m_y;
+			m[2] = m_z;
+		}
+*/
+		SIMD_FORCE_INLINE void	setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+			m_unusedW=w;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord()
+		//	:m_x(btScalar(0.)),m_y(btScalar(0.)),m_z(btScalar(0.)),m_unusedW(btScalar(0.))
+		{
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
+		{
+			*((btQuadWordStorage*)this) = q;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)		
+		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f;
+		}
+
+		SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+		{
+			m_x = x, m_y = y, m_z = z, m_unusedW = w;
+		}
+
+
+		SIMD_FORCE_INLINE void	setMax(const btQuadWord& other)
+		{
+			btSetMax(m_x, other.m_x);
+			btSetMax(m_y, other.m_y);
+			btSetMax(m_z, other.m_z);
+			btSetMax(m_unusedW, other.m_unusedW);
+		}
+
+		SIMD_FORCE_INLINE void	setMin(const btQuadWord& other)
+		{
+			btSetMin(m_x, other.m_x);
+			btSetMin(m_y, other.m_y);
+			btSetMin(m_z, other.m_z);
+			btSetMin(m_unusedW, other.m_unusedW);
+		}
+
+
+
+};
+
+#endif //SIMD_QUADWORD_H
Index: code/branches/physics/src/bullet/LinearMath/btQuaternion.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btQuaternion.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btQuaternion.h	(revision 1963)
@@ -0,0 +1,323 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__QUATERNION_H_
+#define SIMD__QUATERNION_H_
+
+#include "btVector3.h"
+
+///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform.
+class btQuaternion : public btQuadWord {
+public:
+	btQuaternion() {}
+
+	//		template <typename btScalar>
+	//		explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
+
+	btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 
+		: btQuadWord(x, y, z, w) 
+	{}
+
+	btQuaternion(const btVector3& axis, const btScalar& angle) 
+	{ 
+		setRotation(axis, angle); 
+	}
+
+	btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	{ 
+		setEuler(yaw, pitch, roll); 
+	}
+
+	void setRotation(const btVector3& axis, const btScalar& angle)
+	{
+		btScalar d = axis.length();
+		assert(d != btScalar(0.0));
+		btScalar s = btSin(angle * btScalar(0.5)) / d;
+		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
+			btCos(angle * btScalar(0.5)));
+	}
+
+	void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	{
+		btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
+		btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
+		btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
+		btScalar cosYaw = btCos(halfYaw);
+		btScalar sinYaw = btSin(halfYaw);
+		btScalar cosPitch = btCos(halfPitch);
+		btScalar sinPitch = btSin(halfPitch);
+		btScalar cosRoll = btCos(halfRoll);
+		btScalar sinRoll = btSin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+
+	btQuaternion& operator+=(const btQuaternion& q)
+	{
+		m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
+		return *this;
+	}
+
+	btQuaternion& operator-=(const btQuaternion& q) 
+	{
+		m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
+		return *this;
+	}
+
+	btQuaternion& operator*=(const btScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
+		return *this;
+	}
+
+
+	btQuaternion& operator*=(const btQuaternion& q)
+	{
+		setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
+			m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
+			m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
+			m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
+		return *this;
+	}
+
+	btScalar dot(const btQuaternion& q) const
+	{
+		return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
+	}
+
+	btScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	btScalar length() const
+	{
+		return btSqrt(length2());
+	}
+
+	btQuaternion& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator*(const btScalar& s) const
+	{
+		return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
+	}
+
+
+
+	btQuaternion operator/(const btScalar& s) const
+	{
+		assert(s != btScalar(0.0));
+		return *this * (btScalar(1.0) / s);
+	}
+
+
+	btQuaternion& operator/=(const btScalar& s) 
+	{
+		assert(s != btScalar(0.0));
+		return *this *= btScalar(1.0) / s;
+	}
+
+
+	btQuaternion normalized() const 
+	{
+		return *this / length();
+	} 
+
+	btScalar angle(const btQuaternion& q) const 
+	{
+		btScalar s = btSqrt(length2() * q.length2());
+		assert(s != btScalar(0.0));
+		return btAcos(dot(q) / s);
+	}
+
+	btScalar getAngle() const 
+	{
+		btScalar s = btScalar(2.) * btAcos(m_unusedW);
+		return s;
+	}
+
+
+
+	btQuaternion inverse() const
+	{
+		return btQuaternion(-m_x, -m_y, -m_z, m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator+(const btQuaternion& q2) const
+	{
+		const btQuaternion& q1 = *this;
+		return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion
+	operator-(const btQuaternion& q2) const
+	{
+		const btQuaternion& q1 = *this;
+		return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion operator-() const
+	{
+		const btQuaternion& q2 = *this;
+		return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
+	{
+		btQuaternion diff,sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if( diff.dot(diff) > sum.dot(sum) )
+			return qd;
+		return (-qd);
+	}
+
+	btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
+	{
+		btScalar theta = angle(q);
+		if (theta != btScalar(0.0))
+		{
+			btScalar d = btScalar(1.0) / btSin(theta);
+			btScalar s0 = btSin((btScalar(1.0) - t) * theta);
+			btScalar s1 = btSin(t * theta);   
+			return btQuaternion((m_x * s0 + q.x() * s1) * d,
+				(m_y * s0 + q.y() * s1) * d,
+				(m_z * s0 + q.z() * s1) * d,
+				(m_unusedW * s0 + q.m_unusedW * s1) * d);
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
+
+	
+};
+
+
+
+SIMD_FORCE_INLINE btQuaternion
+operator-(const btQuaternion& q)
+{
+	return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
+}
+
+
+
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q1, const btQuaternion& q2) {
+	return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
+		q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
+		q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
+		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btQuaternion& q, const btVector3& w)
+{
+	return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
+		q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
+		q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
+		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
+}
+
+SIMD_FORCE_INLINE btQuaternion
+operator*(const btVector3& w, const btQuaternion& q)
+{
+	return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+		w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+		w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
+		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
+}
+
+SIMD_FORCE_INLINE btScalar 
+dot(const btQuaternion& q1, const btQuaternion& q2) 
+{ 
+	return q1.dot(q2); 
+}
+
+
+SIMD_FORCE_INLINE btScalar
+length(const btQuaternion& q) 
+{ 
+	return q.length(); 
+}
+
+SIMD_FORCE_INLINE btScalar
+angle(const btQuaternion& q1, const btQuaternion& q2) 
+{ 
+	return q1.angle(q2); 
+}
+
+
+SIMD_FORCE_INLINE btQuaternion
+inverse(const btQuaternion& q) 
+{
+	return q.inverse();
+}
+
+SIMD_FORCE_INLINE btQuaternion
+slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
+{
+	return q1.slerp(q2, t);
+}
+
+SIMD_FORCE_INLINE btVector3 
+quatRotate(const btQuaternion& rotation, const btVector3& v) 
+{
+	btQuaternion q = rotation * v;
+	q *= rotation.inverse();
+	return btVector3(q.getX(),q.getY(),q.getZ());
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	btVector3 c = v0.cross(v1);
+	btScalar  d = v0.dot(v1);
+
+	if (d < -1.0 + SIMD_EPSILON)
+		return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
+
+	btScalar  s = btSqrt((1.0f + d) * 2.0f);
+	btScalar rs = 1.0f / s;
+
+	return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return shortestArcQuat(v0,v1);
+}
+
+#endif
+
+
+
+
Index: code/branches/physics/src/bullet/LinearMath/btQuickprof.cpp
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btQuickprof.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btQuickprof.cpp	(revision 1963)
@@ -0,0 +1,282 @@
+/*
+
+/***************************************************************************************************
+**
+** profile.cpp
+**
+** Real-Time Hierarchical Profiling for Game Programming Gems 3
+**
+** by Greg Hjelstrom & Byon Garrabrant
+**
+***************************************************************************************************/
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+#include "LinearMath/btQuickprof.h"
+
+
+#ifdef USE_BT_CLOCK
+
+static btClock gProfileClock;
+
+inline void Profile_Get_Ticks(unsigned long int * ticks)
+{
+	*ticks = gProfileClock.getTimeMicroseconds();
+}
+
+inline float Profile_Get_Tick_Rate(void)
+{
+//	return 1000000.f;
+	return 1000.f;
+
+}
+
+
+
+/***************************************************************************************************
+**
+** CProfileNode
+**
+***************************************************************************************************/
+
+/***********************************************************************************************
+ * INPUT:                                                                                      *
+ * name - pointer to a static string which is the name of this profile node                    *
+ * parent - parent pointer                                                                     *
+ *                                                                                             *
+ * WARNINGS:                                                                                   *
+ * The name is assumed to be a static pointer, only the pointer is stored and compared for     *
+ * efficiency reasons.                                                                         *
+ *=============================================================================================*/
+CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) :
+	Name( name ),
+	TotalCalls( 0 ),
+	TotalTime( 0 ),
+	StartTime( 0 ),
+	RecursionCounter( 0 ),
+	Parent( parent ),
+	Child( NULL ),
+	Sibling( NULL )
+{
+	Reset();
+}
+
+
+void	CProfileNode::CleanupMemory()
+{
+	delete ( Child);
+	Child = NULL;
+	delete ( Sibling);
+	Sibling = NULL;
+}
+
+CProfileNode::~CProfileNode( void )
+{
+	delete ( Child);
+	delete ( Sibling);
+}
+
+
+/***********************************************************************************************
+ * INPUT:                                                                                      *
+ * name - static string pointer to the name of the node we are searching for                   *
+ *                                                                                             *
+ * WARNINGS:                                                                                   *
+ * All profile names are assumed to be static strings so this function uses pointer compares   *
+ * to find the named node.                                                                     *
+ *=============================================================================================*/
+CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
+{
+	// Try to find this sub node
+	CProfileNode * child = Child;
+	while ( child ) {
+		if ( child->Name == name ) {
+			return child;
+		}
+		child = child->Sibling;
+	}
+
+	// We didn't find it, so add it
+	
+	CProfileNode * node = new CProfileNode( name, this );
+	node->Sibling = Child;
+	Child = node;
+	return node;
+}
+
+
+void	CProfileNode::Reset( void )
+{
+	TotalCalls = 0;
+	TotalTime = 0.0f;
+	gProfileClock.reset();
+
+	if ( Child ) {
+		Child->Reset();
+	}
+	if ( Sibling ) {
+		Sibling->Reset();
+	}
+}
+
+
+void	CProfileNode::Call( void )
+{
+	TotalCalls++;
+	if (RecursionCounter++ == 0) {
+		Profile_Get_Ticks(&StartTime);
+	}
+}
+
+
+bool	CProfileNode::Return( void )
+{
+	if ( --RecursionCounter == 0 && TotalCalls != 0 ) { 
+		unsigned long int time;
+		Profile_Get_Ticks(&time);
+		time-=StartTime;
+		TotalTime += (float)time / Profile_Get_Tick_Rate();
+	}
+	return ( RecursionCounter == 0 );
+}
+
+
+/***************************************************************************************************
+**
+** CProfileIterator
+**
+***************************************************************************************************/
+CProfileIterator::CProfileIterator( CProfileNode * start )
+{
+	CurrentParent = start;
+	CurrentChild = CurrentParent->Get_Child();
+}
+
+
+void	CProfileIterator::First(void)
+{
+	CurrentChild = CurrentParent->Get_Child();
+}
+
+
+void	CProfileIterator::Next(void)
+{
+	CurrentChild = CurrentChild->Get_Sibling();
+}
+
+
+bool	CProfileIterator::Is_Done(void)
+{
+	return CurrentChild == NULL;
+}
+
+
+void	CProfileIterator::Enter_Child( int index )
+{
+	CurrentChild = CurrentParent->Get_Child();
+	while ( (CurrentChild != NULL) && (index != 0) ) {
+		index--;
+		CurrentChild = CurrentChild->Get_Sibling();
+	}
+
+	if ( CurrentChild != NULL ) {
+		CurrentParent = CurrentChild;
+		CurrentChild = CurrentParent->Get_Child();
+	}
+}
+
+
+void	CProfileIterator::Enter_Parent( void )
+{
+	if ( CurrentParent->Get_Parent() != NULL ) {
+		CurrentParent = CurrentParent->Get_Parent();
+	}
+	CurrentChild = CurrentParent->Get_Child();
+}
+
+
+/***************************************************************************************************
+**
+** CProfileManager
+**
+***************************************************************************************************/
+
+CProfileNode	CProfileManager::Root( "Root", NULL );
+CProfileNode *	CProfileManager::CurrentNode = &CProfileManager::Root;
+int				CProfileManager::FrameCounter = 0;
+unsigned long int			CProfileManager::ResetTime = 0;
+
+
+/***********************************************************************************************
+ * CProfileManager::Start_Profile -- Begin a named profile                                    *
+ *                                                                                             *
+ * Steps one level deeper into the tree, if a child already exists with the specified name     *
+ * then it accumulates the profiling; otherwise a new child node is added to the profile tree. *
+ *                                                                                             *
+ * INPUT:                                                                                      *
+ * name - name of this profiling record                                                        *
+ *                                                                                             *
+ * WARNINGS:                                                                                   *
+ * The string used is assumed to be a static string; pointer compares are used throughout      *
+ * the profiling code for efficiency.                                                          *
+ *=============================================================================================*/
+void	CProfileManager::Start_Profile( const char * name )
+{
+	if (name != CurrentNode->Get_Name()) {
+		CurrentNode = CurrentNode->Get_Sub_Node( name );
+	} 
+	
+	CurrentNode->Call();
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Stop_Profile -- Stop timing and record the results.                       *
+ *=============================================================================================*/
+void	CProfileManager::Stop_Profile( void )
+{
+	// Return will indicate whether we should back up to our parent (we may
+	// be profiling a recursive function)
+	if (CurrentNode->Return()) {
+		CurrentNode = CurrentNode->Get_Parent();
+	}
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Reset -- Reset the contents of the profiling system                       *
+ *                                                                                             *
+ *    This resets everything except for the tree structure.  All of the timing data is reset.  *
+ *=============================================================================================*/
+void	CProfileManager::Reset( void )
+{ 
+	Root.Reset();
+    Root.Call();
+	FrameCounter = 0;
+	Profile_Get_Ticks(&ResetTime);
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Increment_Frame_Counter -- Increment the frame counter                    *
+ *=============================================================================================*/
+void CProfileManager::Increment_Frame_Counter( void )
+{
+	FrameCounter++;
+}
+
+
+/***********************************************************************************************
+ * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset         *
+ *=============================================================================================*/
+float CProfileManager::Get_Time_Since_Reset( void )
+{
+	unsigned long int time;
+	Profile_Get_Ticks(&time);
+	time -= ResetTime;
+	return (float)time / Profile_Get_Tick_Rate();
+}
+
+#endif //USE_BT_CLOCK
+
Index: code/branches/physics/src/bullet/LinearMath/btQuickprof.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btQuickprof.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btQuickprof.h	(revision 1963)
@@ -0,0 +1,358 @@
+
+/***************************************************************************************************
+**
+** Real-Time Hierarchical Profiling for Game Programming Gems 3
+**
+** by Greg Hjelstrom & Byon Garrabrant
+**
+***************************************************************************************************/
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+#ifndef QUICK_PROF_H
+#define QUICK_PROF_H
+
+#include "btScalar.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include <new>
+//To disable built-in profiling, please comment out next line
+//#define BT_NO_PROFILE 1
+
+
+//if you don't need btClock, you can comment next line
+#define USE_BT_CLOCK 1
+
+#ifdef USE_BT_CLOCK
+#ifdef __CELLOS_LV2__
+#include <sys/sys_time.h>
+#include <sys/time_util.h>
+#include <stdio.h>
+#endif
+
+#if defined (SUNOS) || defined (__SUNOS__) 
+#include <stdio.h> 
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+
+#define USE_WINDOWS_TIMERS 
+#define WIN32_LEAN_AND_MEAN 
+#define NOWINRES 
+#define NOMCX 
+#define NOIME 
+#ifdef _XBOX
+#include <Xtl.h>
+#else
+#include <windows.h>
+#endif
+#include <time.h>
+
+#else
+#include <sys/time.h>
+#endif
+
+#define mymin(a,b) (a > b ? a : b)
+
+///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
+class btClock
+{
+public:
+	btClock()
+	{
+#ifdef USE_WINDOWS_TIMERS
+		QueryPerformanceFrequency(&mClockFrequency);
+#endif
+		reset();
+	}
+
+	~btClock()
+	{
+	}
+
+	/// Resets the initial reference time.
+	void reset()
+	{
+#ifdef USE_WINDOWS_TIMERS
+		QueryPerformanceCounter(&mStartTime);
+		mStartTick = GetTickCount();
+		mPrevElapsedTime = 0;
+#else
+#ifdef __CELLOS_LV2__
+
+		typedef uint64_t  ClockSize;
+		ClockSize newTime;
+		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+		SYS_TIMEBASE_GET( newTime );
+		mStartTime = newTime;
+#else
+		gettimeofday(&mStartTime, 0);
+#endif
+
+#endif
+	}
+
+	/// Returns the time in ms since the last call to reset or since 
+	/// the btClock was created.
+	unsigned long int getTimeMilliseconds()
+	{
+#ifdef USE_WINDOWS_TIMERS
+		LARGE_INTEGER currentTime;
+		QueryPerformanceCounter(&currentTime);
+		LONGLONG elapsedTime = currentTime.QuadPart - 
+			mStartTime.QuadPart;
+
+		// Compute the number of millisecond ticks elapsed.
+		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+			mClockFrequency.QuadPart);
+
+		// Check for unexpected leaps in the Win32 performance counter.  
+		// (This is caused by unexpected data across the PCI to ISA 
+		// bridge, aka south bridge.  See Microsoft KB274323.)
+		unsigned long elapsedTicks = GetTickCount() - mStartTick;
+		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+		if (msecOff < -100 || msecOff > 100)
+		{
+			// Adjust the starting time forwards.
+			LONGLONG msecAdjustment = mymin(msecOff * 
+				mClockFrequency.QuadPart / 1000, elapsedTime - 
+				mPrevElapsedTime);
+			mStartTime.QuadPart += msecAdjustment;
+			elapsedTime -= msecAdjustment;
+
+			// Recompute the number of millisecond ticks elapsed.
+			msecTicks = (unsigned long)(1000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+		}
+
+		// Store the current elapsed time for adjustments next time.
+		mPrevElapsedTime = elapsedTime;
+
+		return msecTicks;
+#else
+
+#ifdef __CELLOS_LV2__
+		uint64_t freq=sys_time_get_timebase_frequency();
+		double dFreq=((double) freq) / 1000.0;
+		typedef uint64_t  ClockSize;
+		ClockSize newTime;
+		SYS_TIMEBASE_GET( newTime );
+		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+
+		return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+
+		struct timeval currentTime;
+		gettimeofday(&currentTime, 0);
+		return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + 
+			(currentTime.tv_usec - mStartTime.tv_usec) / 1000;
+#endif //__CELLOS_LV2__
+#endif
+	}
+
+	/// Returns the time in us since the last call to reset or since 
+	/// the Clock was created.
+	unsigned long int getTimeMicroseconds()
+	{
+#ifdef USE_WINDOWS_TIMERS
+		LARGE_INTEGER currentTime;
+		QueryPerformanceCounter(&currentTime);
+		LONGLONG elapsedTime = currentTime.QuadPart - 
+			mStartTime.QuadPart;
+
+		// Compute the number of millisecond ticks elapsed.
+		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+			mClockFrequency.QuadPart);
+
+		// Check for unexpected leaps in the Win32 performance counter.  
+		// (This is caused by unexpected data across the PCI to ISA 
+		// bridge, aka south bridge.  See Microsoft KB274323.)
+		unsigned long elapsedTicks = GetTickCount() - mStartTick;
+		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+		if (msecOff < -100 || msecOff > 100)
+		{
+			// Adjust the starting time forwards.
+			LONGLONG msecAdjustment = mymin(msecOff * 
+				mClockFrequency.QuadPart / 1000, elapsedTime - 
+				mPrevElapsedTime);
+			mStartTime.QuadPart += msecAdjustment;
+			elapsedTime -= msecAdjustment;
+		}
+
+		// Store the current elapsed time for adjustments next time.
+		mPrevElapsedTime = elapsedTime;
+
+		// Convert to microseconds.
+		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / 
+			mClockFrequency.QuadPart);
+
+		return usecTicks;
+#else
+
+#ifdef __CELLOS_LV2__
+		uint64_t freq=sys_time_get_timebase_frequency();
+		double dFreq=((double) freq)/ 1000000.0;
+		typedef uint64_t  ClockSize;
+		ClockSize newTime;
+		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+		SYS_TIMEBASE_GET( newTime );
+
+		return (unsigned long int)((double(newTime-mStartTime)) / dFreq);
+#else
+
+		struct timeval currentTime;
+		gettimeofday(&currentTime, 0);
+		return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + 
+			(currentTime.tv_usec - mStartTime.tv_usec);
+#endif//__CELLOS_LV2__
+#endif 
+	}
+
+private:
+#ifdef USE_WINDOWS_TIMERS
+	LARGE_INTEGER mClockFrequency;
+	DWORD mStartTick;
+	LONGLONG mPrevElapsedTime;
+	LARGE_INTEGER mStartTime;
+#else
+#ifdef __CELLOS_LV2__
+	uint64_t	mStartTime;
+#else
+	struct timeval mStartTime;
+#endif
+#endif //__CELLOS_LV2__
+
+};
+
+#endif //USE_BT_CLOCK
+
+
+
+
+///A node in the Profile Hierarchy Tree
+class	CProfileNode {
+
+public:
+	CProfileNode( const char * name, CProfileNode * parent );
+	~CProfileNode( void );
+
+	CProfileNode * Get_Sub_Node( const char * name );
+
+	CProfileNode * Get_Parent( void )		{ return Parent; }
+	CProfileNode * Get_Sibling( void )		{ return Sibling; }
+	CProfileNode * Get_Child( void )			{ return Child; }
+
+	void				CleanupMemory();
+	void				Reset( void );
+	void				Call( void );
+	bool				Return( void );
+
+	const char *	Get_Name( void )				{ return Name; }
+	int				Get_Total_Calls( void )		{ return TotalCalls; }
+	float				Get_Total_Time( void )		{ return TotalTime; }
+
+protected:
+
+	const char *	Name;
+	int				TotalCalls;
+	float				TotalTime;
+	unsigned long int			StartTime;
+	int				RecursionCounter;
+
+	CProfileNode *	Parent;
+	CProfileNode *	Child;
+	CProfileNode *	Sibling;
+};
+
+///An iterator to navigate through the tree
+class CProfileIterator
+{
+public:
+	// Access all the children of the current parent
+	void				First(void);
+	void				Next(void);
+	bool				Is_Done(void);
+	bool                Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
+
+	void				Enter_Child( int index );		// Make the given child the new parent
+	void				Enter_Largest_Child( void );	// Make the largest child the new parent
+	void				Enter_Parent( void );			// Make the current parent's parent the new parent
+
+	// Access the current child
+	const char *	Get_Current_Name( void )			{ return CurrentChild->Get_Name(); }
+	int				Get_Current_Total_Calls( void )	{ return CurrentChild->Get_Total_Calls(); }
+	float				Get_Current_Total_Time( void )	{ return CurrentChild->Get_Total_Time(); }
+
+	// Access the current parent
+	const char *	Get_Current_Parent_Name( void )			{ return CurrentParent->Get_Name(); }
+	int				Get_Current_Parent_Total_Calls( void )	{ return CurrentParent->Get_Total_Calls(); }
+	float				Get_Current_Parent_Total_Time( void )	{ return CurrentParent->Get_Total_Time(); }
+
+protected:
+
+	CProfileNode *	CurrentParent;
+	CProfileNode *	CurrentChild;
+
+	CProfileIterator( CProfileNode * start );
+	friend	class		CProfileManager;
+};
+
+
+///The Manager for the Profile system
+class	CProfileManager {
+public:
+	static	void						Start_Profile( const char * name );
+	static	void						Stop_Profile( void );
+
+	static	void						CleanupMemory(void)
+	{
+		Root.CleanupMemory();
+	}
+
+	static	void						Reset( void );
+	static	void						Increment_Frame_Counter( void );
+	static	int						Get_Frame_Count_Since_Reset( void )		{ return FrameCounter; }
+	static	float						Get_Time_Since_Reset( void );
+
+	static	CProfileIterator *	Get_Iterator( void )	
+	{ 
+		
+		return new CProfileIterator( &Root ); 
+	}
+	static	void						Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
+
+private:
+	static	CProfileNode			Root;
+	static	CProfileNode *			CurrentNode;
+	static	int						FrameCounter;
+	static	unsigned long int					ResetTime;
+};
+
+
+///ProfileSampleClass is a simple way to profile a function's scope
+///Use the BT_PROFILE macro at the start of scope to time
+class	CProfileSample {
+public:
+	CProfileSample( const char * name )
+	{ 
+		CProfileManager::Start_Profile( name ); 
+	}
+
+	~CProfileSample( void )					
+	{ 
+		CProfileManager::Stop_Profile(); 
+	}
+};
+
+#if !defined(BT_NO_PROFILE)
+#define	BT_PROFILE( name )			CProfileSample __profile( name )
+#else
+#define	BT_PROFILE( name )
+#endif
+
+
+
+
+#endif //QUICK_PROF_H
+
+
Index: code/branches/physics/src/bullet/LinearMath/btRandom.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btRandom.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btRandom.h	(revision 1963)
@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_RANDOM_H
+#define GEN_RANDOM_H
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define GEN_RAND_MAX UINT_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { init_genrand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define GEN_RAND_MAX RAND_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { srand(seed); } 
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return rand(); }
+
+#endif
+
+#endif
+
Index: code/branches/physics/src/bullet/LinearMath/btScalar.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btScalar.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btScalar.h	(revision 1963)
@@ -0,0 +1,421 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD___SCALAR_H
+#define SIMD___SCALAR_H
+
+#include <math.h>
+#include <stdlib.h>//size_t for MSVC 6.0
+
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+#define BT_BULLET_VERSION 273
+
+inline int	btGetVersion()
+{
+	return BT_BULLET_VERSION;
+}
+
+#if defined(DEBUG) || defined (_DEBUG)
+#define BT_DEBUG
+#endif
+
+
+#ifdef WIN32
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+
+			#define SIMD_FORCE_INLINE inline
+			#define ATTRIBUTE_ALIGNED16(a) a
+			#define ATTRIBUTE_ALIGNED128(a) a
+		#else
+			#define BT_HAS_ALIGNED_ALLOCATOR
+			#pragma warning(disable : 4324) // disable padding warning
+//			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
+//			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+//			#pragma warning(disable:4786) // Disable the "debug name too long" warning
+
+			#define SIMD_FORCE_INLINE __forceinline
+			#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+			#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
+		#ifdef _XBOX
+			#define BT_USE_VMX128
+
+			#include <ppcintrinsics.h>
+ 			#define BT_HAVE_NATIVE_FSEL
+ 			#define btFsel(a,b,c) __fsel((a),(b),(c))
+		#else
+			#define BT_USE_SSE
+		#endif
+		#endif //__MINGW32__
+
+		#include <assert.h>
+#if defined(DEBUG) || defined (_DEBUG)
+		#define btAssert assert
+#else
+		#define btAssert(x)
+#endif
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+
+		#define btLikely(_c)  _c
+		#define btUnlikely(_c) _c
+
+#else
+	
+#if defined	(__CELLOS_LV2__)
+		#define SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+		#define btAssert assert
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+
+		#define btLikely(_c)  _c
+		#define btUnlikely(_c) _c
+
+#else
+
+#ifdef USE_LIBSPE2
+
+		#define SIMD_FORCE_INLINE __inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+		#define btAssert assert
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+
+
+		#define btLikely(_c)   __builtin_expect((_c), 1)
+		#define btUnlikely(_c) __builtin_expect((_c), 0)
+		
+
+#else
+	//non-windows systems
+
+		#define SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a
+		#define ATTRIBUTE_ALIGNED128(a) a
+		#ifndef assert
+		#include <assert.h>
+		#endif
+
+#if defined(DEBUG) || defined (_DEBUG)
+		#define btAssert assert
+#else
+		#define btAssert(x)
+#endif
+
+		//btFullAssert is optional, slows down a lot
+		#define btFullAssert(x)
+		#define btLikely(_c)  _c
+		#define btUnlikely(_c) _c
+
+
+#endif // LIBSPE2
+
+#endif	//__CELLOS_LV2__
+#endif
+
+/// older compilers (gcc 3.x) and Sun needs double version of sqrt etc.
+/// exclude Apple Intel (i's assumed to be a Macbook or new Intel Dual Core Processor)
+#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
+//use slow double float precision operation on those platforms
+#ifndef BT_USE_DOUBLE_PRECISION
+#define BT_FORCE_DOUBLE_FUNCTIONS
+#endif
+#endif
+
+///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
+#if defined(BT_USE_DOUBLE_PRECISION)
+typedef double btScalar;
+#else
+typedef float btScalar;
+#endif
+
+
+
+#define BT_DECLARE_ALIGNED_ALLOCATOR() \
+   SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
+   SIMD_FORCE_INLINE void  operator delete(void* ptr)         { btAlignedFree(ptr); }   \
+   SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
+   SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
+   SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
+   SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { btAlignedFree(ptr); }   \
+   SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
+   SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
+
+
+
+#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
+		
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); }
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
+
+#else
+		
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) 
+{ 
+#ifdef USE_APPROXIMATION
+    double x, z, tempf;
+    unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
+
+	tempf = y;
+	*tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */
+	x =  tempf;
+	z =  y*btScalar(0.5);                        /* hoist out the /2    */
+	x = (btScalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
+	x = (btScalar(1.5)*x)-(x*x)*(x*z);
+	x = (btScalar(1.5)*x)-(x*x)*(x*z);
+	x = (btScalar(1.5)*x)-(x*x)*(x*z);
+	x = (btScalar(1.5)*x)-(x*x)*(x*z);
+	return x*y;
+#else
+	return sqrtf(y); 
+#endif
+}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { 
+	btAssert(x <= btScalar(1.));
+	return acosf(x); 
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+	
+#endif
+
+#define SIMD_2_PI         btScalar(6.283185307179586232)
+#define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
+#define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
+#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
+#define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define SIMD_EPSILON      DBL_EPSILON
+#define SIMD_INFINITY     DBL_MAX
+#else
+#define SIMD_EPSILON      FLT_EPSILON
+#define SIMD_INFINITY     FLT_MAX
+#endif
+
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
+{
+	btScalar coeff_1 = SIMD_PI / 4.0f;
+	btScalar coeff_2 = 3.0f * coeff_1;
+	btScalar abs_y = btFabs(y);
+	btScalar angle;
+	if (x >= 0.0f) {
+		btScalar r = (x - abs_y) / (x + abs_y);
+		angle = coeff_1 - coeff_1 * r;
+	} else {
+		btScalar r = (x + abs_y) / (abs_y - x);
+		angle = coeff_2 - coeff_1 * r;
+	}
+	return (y < 0.0f) ? -angle : angle;
+}
+
+SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
+
+SIMD_FORCE_INLINE bool	btEqual(btScalar a, btScalar eps) {
+	return (((a) <= eps) && !((a) < -eps));
+}
+SIMD_FORCE_INLINE bool	btGreaterEqual (btScalar a, btScalar eps) {
+	return (!((a) <= eps));
+}
+
+
+SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
+    return x < btScalar(0.0) ? 1 : 0;
+}
+
+SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
+
+#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifndef btFsel
+SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
+{
+	return a >= 0 ? b : c;
+}
+#endif
+#define btFsels(a,b,c) (btScalar)btFsel(a,b,c)
+
+
+SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
+{
+   long int i = 1;
+   const char *p = (const char *) &i;
+   if (p[0] == 1)  // Lowest address contains the least significant byte
+	   return true;
+   else
+	   return false;
+}
+
+
+
+///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
+{
+    // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+    // Rely on positive value or'ed with its negative having sign bit on
+    // and zero value or'ed with its negative (which is still zero) having sign bit off 
+    // Use arithmetic shift right, shifting the sign bit through all 32 bits
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz;
+    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
+}
+SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz; 
+    return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef BT_HAVE_NATIVE_FSEL
+    return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
+#endif
+}
+
+template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+
+//PCK: endian swapping functions
+SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
+{
+	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
+{
+	return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+}
+
+SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
+{
+	return btSwapEndian((unsigned)val);
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
+{
+	return btSwapEndian((unsigned short) val);
+}
+
+///btSwapFloat uses using char pointers to swap the endianness
+////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. 
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. 
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. 
+///so instead of returning a float/double, we return integer/long long integer
+SIMD_FORCE_INLINE unsigned int  btSwapEndianFloat(float d)
+{
+    unsigned int a = 0;
+    unsigned char *dst = (unsigned char *)&a;
+    unsigned char *src = (unsigned char *)&d;
+
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
+    return a;
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) 
+{
+    float d = 0.0f;
+    unsigned char *src = (unsigned char *)&a;
+    unsigned char *dst = (unsigned char *)&d;
+
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
+
+    return d;
+}
+
+
+// swap using char pointers
+SIMD_FORCE_INLINE void  btSwapEndianDouble(double d, unsigned char* dst)
+{
+    unsigned char *src = (unsigned char *)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
+
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) 
+{
+    double d = 0.0;
+    unsigned char *dst = (unsigned char *)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
+
+	return d;
+}
+
+
+#endif //SIMD___SCALAR_H
Index: code/branches/physics/src/bullet/LinearMath/btStackAlloc.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btStackAlloc.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btStackAlloc.h	(revision 1963)
@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+#ifndef BT_STACK_ALLOC
+#define BT_STACK_ALLOC
+
+#include "btScalar.h" //for btAssert
+#include "btAlignedAllocator.h"
+
+struct btBlock
+{
+	btBlock*			previous;
+	unsigned char*		address;
+};
+
+///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
+class btStackAlloc
+{
+public:
+
+	btStackAlloc(unsigned int size)	{ ctor();create(size); }
+	~btStackAlloc()		{ destroy(); }
+	
+	inline void		create(unsigned int size)
+	{
+		destroy();
+		data		=  (unsigned char*) btAlignedAlloc(size,16);
+		totalsize	=	size;
+	}
+	inline void		destroy()
+	{
+		btAssert(usedsize==0);
+		//Raise(L"StackAlloc is still in use");
+
+		if(usedsize==0)
+		{
+			if(!ischild && data)		
+				btAlignedFree(data);
+
+			data				=	0;
+			usedsize			=	0;
+		}
+		
+	}
+
+	int	getAvailableMemory() const
+	{
+		return static_cast<int>(totalsize - usedsize);
+	}
+
+	unsigned char*			allocate(unsigned int size)
+	{
+		const unsigned int	nus(usedsize+size);
+		if(nus<totalsize)
+		{
+			usedsize=nus;
+			return(data+(usedsize-size));
+		}
+		btAssert(0);
+		//&& (L"Not enough memory"));
+		
+		return(0);
+	}
+	SIMD_FORCE_INLINE btBlock*		beginBlock()
+	{
+		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
+		pb->previous	=	current;
+		pb->address		=	data+usedsize;
+		current			=	pb;
+		return(pb);
+	}
+	SIMD_FORCE_INLINE void		endBlock(btBlock* block)
+	{
+		btAssert(block==current);
+		//Raise(L"Unmatched blocks");
+		if(block==current)
+		{
+			current		=	block->previous;
+			usedsize	=	(unsigned int)((block->address-data)-sizeof(btBlock));
+		}
+	}
+
+private:
+	void		ctor()
+	{
+		data		=	0;
+		totalsize	=	0;
+		usedsize	=	0;
+		current		=	0;
+		ischild		=	false;
+	}
+	unsigned char*		data;
+	unsigned int		totalsize;
+	unsigned int		usedsize;
+	btBlock*	current;
+	bool		ischild;
+};
+
+#endif //BT_STACK_ALLOC
Index: code/branches/physics/src/bullet/LinearMath/btTransform.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btTransform.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btTransform.h	(revision 1963)
@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef btTransform_H
+#define btTransform_H
+
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+
+
+///The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
+///It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes.
+class btTransform {
+	
+
+public:
+	
+	
+	btTransform() {}
+
+	explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, 
+		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) 
+		: m_basis(q),
+		m_origin(c)
+	{}
+
+	explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, 
+		const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
+		: m_basis(b),
+		m_origin(c)
+	{}
+
+	SIMD_FORCE_INLINE btTransform (const btTransform& other)
+		: m_basis(other.m_basis),
+		m_origin(other.m_origin)
+	{
+	}
+
+	SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
+	{
+		m_basis = other.m_basis;
+		m_origin = other.m_origin;
+		return *this;
+	}
+
+
+		SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
+			m_basis = t1.m_basis * t2.m_basis;
+			m_origin = t1(t2.m_origin);
+		}
+
+/*		void multInverseLeft(const btTransform& t1, const btTransform& t2) {
+			btVector3 v = t2.m_origin - t1.m_origin;
+			m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
+			m_origin = v * t1.m_basis;
+		}
+		*/
+
+
+	SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
+	{
+		return btVector3(m_basis[0].dot(x) + m_origin.x(), 
+			m_basis[1].dot(x) + m_origin.y(), 
+			m_basis[2].dot(x) + m_origin.z());
+	}
+
+	SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
+	{
+		return (*this)(x);
+	}
+
+	SIMD_FORCE_INLINE btMatrix3x3&       getBasis()          { return m_basis; }
+	SIMD_FORCE_INLINE const btMatrix3x3& getBasis()    const { return m_basis; }
+
+	SIMD_FORCE_INLINE btVector3&         getOrigin()         { return m_origin; }
+	SIMD_FORCE_INLINE const btVector3&   getOrigin()   const { return m_origin; }
+
+	btQuaternion getRotation() const { 
+		btQuaternion q;
+		m_basis.getRotation(q);
+		return q;
+	}
+	
+	
+	void setFromOpenGLMatrix(const btScalar *m)
+	{
+		m_basis.setFromOpenGLSubMatrix(m);
+		m_origin.setValue(m[12],m[13],m[14]);
+	}
+
+	void getOpenGLMatrix(btScalar *m) const 
+	{
+		m_basis.getOpenGLSubMatrix(m);
+		m[12] = m_origin.x();
+		m[13] = m_origin.y();
+		m[14] = m_origin.z();
+		m[15] = btScalar(1.0);
+	}
+
+	SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) 
+	{ 
+		m_origin = origin;
+	}
+
+	SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
+
+
+
+	SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
+	{ 
+		m_basis = basis;
+	}
+
+	SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
+	{
+		m_basis.setRotation(q);
+	}
+
+
+
+	void setIdentity()
+	{
+		m_basis.setIdentity();
+		m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	}
+
+	
+	btTransform& operator*=(const btTransform& t) 
+	{
+		m_origin += m_basis * t.m_origin;
+		m_basis *= t.m_basis;
+		return *this;
+	}
+
+	btTransform inverse() const
+	{ 
+		btMatrix3x3 inv = m_basis.transpose();
+		return btTransform(inv, inv * -m_origin);
+	}
+
+	btTransform inverseTimes(const btTransform& t) const;  
+
+	btTransform operator*(const btTransform& t) const;
+
+	static btTransform	getIdentity()
+	{
+		btTransform tr;
+		tr.setIdentity();
+		return tr;
+	}
+	
+private:
+
+	btMatrix3x3 m_basis;
+	btVector3   m_origin;
+};
+
+
+SIMD_FORCE_INLINE btVector3
+btTransform::invXform(const btVector3& inVec) const
+{
+	btVector3 v = inVec - m_origin;
+	return (m_basis.transpose() * v);
+}
+
+SIMD_FORCE_INLINE btTransform 
+btTransform::inverseTimes(const btTransform& t) const  
+{
+	btVector3 v = t.getOrigin() - m_origin;
+		return btTransform(m_basis.transposeTimes(t.m_basis),
+			v * m_basis);
+}
+
+SIMD_FORCE_INLINE btTransform 
+btTransform::operator*(const btTransform& t) const
+{
+	return btTransform(m_basis * t.m_basis, 
+		(*this)(t.m_origin));
+}
+
+SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
+{
+   return ( t1.getBasis()  == t2.getBasis() &&
+            t1.getOrigin() == t2.getOrigin() );
+}
+
+
+#endif
+
+
+
+
+
+
Index: code/branches/physics/src/bullet/LinearMath/btTransformUtil.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btTransformUtil.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btTransformUtil.h	(revision 1963)
@@ -0,0 +1,144 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_TRANSFORM_UTIL_H
+#define SIMD_TRANSFORM_UTIL_H
+
+#include "btTransform.h"
+#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
+
+
+
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x))))		/* reciprocal square root */
+
+SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
+{
+	return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
+      supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
+      supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z()); 
+}
+
+
+SIMD_FORCE_INLINE void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q)
+{
+  if (btFabs(n.z()) > SIMDSQRT12) {
+    // choose p in y-z plane
+    btScalar a = n[1]*n[1] + n[2]*n[2];
+    btScalar k = btRecipSqrt (a);
+    p.setValue(0,-n[2]*k,n[1]*k);
+    // set q = n x p
+    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
+  }
+  else {
+    // choose p in x-y plane
+    btScalar a = n.x()*n.x() + n.y()*n.y();
+    btScalar k = btRecipSqrt (a);
+    p.setValue(-n.y()*k,n.x()*k,0);
+    // set q = n x p
+    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
+  }
+}
+
+
+
+/// Utils related to temporal transforms
+class btTransformUtil
+{
+
+public:
+
+	static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
+	{
+		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
+//	#define QUATERNION_DERIVATIVE
+	#ifdef QUATERNION_DERIVATIVE
+		btQuaternion predictedOrn = curTrans.getRotation();
+		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
+		predictedOrn.normalize();
+	#else
+		//Exponential map
+		//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
+
+		btVector3 axis;
+		btScalar	fAngle = angvel.length(); 
+		//limit the angular motion
+		if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
+		{
+			fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
+		}
+
+		if ( fAngle < btScalar(0.001) )
+		{
+			// use Taylor's expansions of sync function
+			axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
+		}
+		else
+		{
+			// sync(fAngle) = sin(c*fAngle)/t
+			axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
+		}
+		btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
+		btQuaternion orn0 = curTrans.getRotation();
+
+		btQuaternion predictedOrn = dorn * orn0;
+		predictedOrn.normalize();
+	#endif
+		predictedTransform.setRotation(predictedOrn);
+	}
+
+	static void	calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
+	{
+		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
+		btVector3 axis;
+		btScalar  angle;
+		calculateDiffAxisAngle(transform0,transform1,axis,angle);
+		angVel = axis * angle / timeStep;
+	}
+
+	static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
+	{
+	
+	#ifdef USE_QUATERNION_DIFF
+		btQuaternion orn0 = transform0.getRotation();
+		btQuaternion orn1a = transform1.getRotation();
+		btQuaternion orn1 = orn0.farthest(orn1a);
+		btQuaternion dorn = orn1 * orn0.inverse();
+#else
+		btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
+		btQuaternion dorn;
+		dmat.getRotation(dorn);
+#endif//USE_QUATERNION_DIFF
+	
+		///floating point inaccuracy can lead to w component > 1..., which breaks 
+
+		dorn.normalize();
+		
+		angle = dorn.getAngle();
+		axis = btVector3(dorn.x(),dorn.y(),dorn.z());
+		axis[3] = btScalar(0.);
+		//check for axis length
+		btScalar len = axis.length2();
+		if (len < SIMD_EPSILON*SIMD_EPSILON)
+			axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
+		else
+			axis /= btSqrt(len);
+	}
+
+};
+
+#endif //SIMD_TRANSFORM_UTIL_H
+
Index: code/branches/physics/src/bullet/LinearMath/btVector3.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btVector3.h	(revision 1963)
+++ code/branches/physics/src/bullet/LinearMath/btVector3.h	(revision 1963)
@@ -0,0 +1,453 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__VECTOR3_H
+#define SIMD__VECTOR3_H
+
+#include "btQuadWord.h"
+
+///btVector3 can be used to represent 3D points and vectors.
+///It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+///Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+class	btVector3 : public btQuadWord {
+
+public:
+	SIMD_FORCE_INLINE btVector3() {}
+
+	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
+		: btQuadWord(q)
+	{
+	}
+	
+
+	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
+		:btQuadWord(x,y,z,btScalar(0.))
+	{
+	}
+
+//	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+//		: btQuadWord(x,y,z,w)
+//	{
+//	}
+
+	
+
+	SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
+	{
+
+		m_x += v.x(); m_y += v.y(); m_z += v.z();
+		return *this;
+	}
+
+
+
+	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
+	{
+		m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s;
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
+	{
+		btFullAssert(s != btScalar(0.0));
+		return *this *= btScalar(1.0) / s;
+	}
+
+	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
+	{
+		return m_x * v.x() + m_y * v.y() + m_z * v.z();
+	}
+
+	SIMD_FORCE_INLINE btScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	SIMD_FORCE_INLINE btScalar length() const
+	{
+		return btSqrt(length2());
+	}
+
+	SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
+
+	SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+
+	SIMD_FORCE_INLINE btVector3& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE btVector3 normalized() const;
+
+	SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle );
+
+	SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
+	{
+		btScalar s = btSqrt(length2() * v.length2());
+		btFullAssert(s != btScalar(0.0));
+		return btAcos(dot(v) / s);
+	}
+
+	SIMD_FORCE_INLINE btVector3 absolute() const 
+	{
+		return btVector3(
+			btFabs(m_x), 
+			btFabs(m_y), 
+			btFabs(m_z));
+	}
+
+	SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
+	{
+		return btVector3(
+			m_y * v.z() - m_z * v.y(),
+			m_z * v.x() - m_x * v.z(),
+			m_x * v.y() - m_y * v.x());
+	}
+
+	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
+	{
+		return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + 
+			m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + 
+			m_z * (v1.x() * v2.y() - v1.y() * v2.x());
+	}
+
+	SIMD_FORCE_INLINE int minAxis() const
+	{
+		return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
+	}
+
+	SIMD_FORCE_INLINE int maxAxis() const 
+	{
+		return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
+	}
+
+	SIMD_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	SIMD_FORCE_INLINE int closestAxis() const 
+	{
+		return absolute().maxAxis();
+	}
+
+	SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
+	{
+		btScalar s = btScalar(1.0) - rt;
+		m_x = s * v0.x() + rt * v1.x();
+		m_y = s * v0.y() + rt * v1.y();
+		m_z = s * v0.z() + rt * v1.z();
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
+	{
+		return btVector3(m_x + (v.x() - m_x) * t,
+			m_y + (v.y() - m_y) * t,
+			m_z + (v.z() - m_z) * t);
+	}
+
+
+	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
+	{
+		m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
+		return *this;
+	}
+
+	
+
+};
+
+SIMD_FORCE_INLINE btVector3 
+operator+(const btVector3& v1, const btVector3& v2) 
+{
+	return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btVector3& v1, const btVector3& v2) 
+{
+	return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator-(const btVector3& v1, const btVector3& v2)
+{
+	return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator-(const btVector3& v)
+{
+	return btVector3(-v.x(), -v.y(), -v.z());
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btVector3& v, const btScalar& s)
+{
+	return btVector3(v.x() * s, v.y() * s, v.z() * s);
+}
+
+SIMD_FORCE_INLINE btVector3 
+operator*(const btScalar& s, const btVector3& v)
+{ 
+	return v * s; 
+}
+
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v, const btScalar& s)
+{
+	btFullAssert(s != btScalar(0.0));
+	return v * (btScalar(1.0) / s);
+}
+
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v1, const btVector3& v2)
+{
+	return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+}
+
+SIMD_FORCE_INLINE btScalar 
+dot(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.dot(v2); 
+}
+
+
+
+SIMD_FORCE_INLINE btScalar
+distance2(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.distance2(v2); 
+}
+
+
+SIMD_FORCE_INLINE btScalar
+distance(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.distance(v2); 
+}
+
+SIMD_FORCE_INLINE btScalar
+angle(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.angle(v2); 
+}
+
+SIMD_FORCE_INLINE btVector3 
+cross(const btVector3& v1, const btVector3& v2) 
+{ 
+	return v1.cross(v2); 
+}
+
+SIMD_FORCE_INLINE btScalar
+triple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+SIMD_FORCE_INLINE btVector3 
+lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+
+SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
+{
+	return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
+{
+	return (v - *this).length2();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
+{
+	return (v - *this).length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
+{
+	return *this / length();
+} 
+
+SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle )
+{
+	// wAxis must be a unit lenght vector
+
+	btVector3 o = wAxis * wAxis.dot( *this );
+	btVector3 x = *this - o;
+	btVector3 y;
+
+	y = wAxis.cross( *this );
+
+	return ( o + x * btCos( angle ) + y * btSin( angle ) );
+}
+
+class btVector4 : public btVector3
+{
+public:
+
+	SIMD_FORCE_INLINE btVector4() {}
+
+
+	SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
+		: btVector3(x,y,z)
+	{
+		m_unusedW = w;
+	}
+
+
+	SIMD_FORCE_INLINE btVector4 absolute4() const 
+	{
+		return btVector4(
+			btFabs(m_x), 
+			btFabs(m_y), 
+			btFabs(m_z),
+			btFabs(m_unusedW));
+	}
+
+
+
+	btScalar	getW() const { return m_unusedW;}
+
+
+		SIMD_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		btScalar maxVal = btScalar(-1e30);
+		if (m_x > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_x;
+		}
+		if (m_y > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_y;
+		}
+		if (m_z > maxVal)
+		{
+			maxIndex = 2;
+			maxVal = m_z;
+		}
+		if (m_unusedW > maxVal)
+		{
+			maxIndex = 3;
+			maxVal = m_unusedW;
+		}
+		
+		
+		
+
+		return maxIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		btScalar minVal = btScalar(1e30);
+		if (m_x < minVal)
+		{
+			minIndex = 0;
+			minVal = m_x;
+		}
+		if (m_y < minVal)
+		{
+			minIndex = 1;
+			minVal = m_y;
+		}
+		if (m_z < minVal)
+		{
+			minIndex = 2;
+			minVal = m_z;
+		}
+		if (m_unusedW < minVal)
+		{
+			minIndex = 3;
+			minVal = m_unusedW;
+		}
+		
+		return minIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int closestAxis4() const 
+	{
+		return absolute4().maxAxis4();
+	}
+
+};
+
+
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void	btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
+{
+	#ifdef BT_USE_DOUBLE_PRECISION
+	unsigned char* dest = (unsigned char*) &destVal;
+	unsigned char* src  = (unsigned char*) &sourceVal;
+	dest[0] = src[7];
+    dest[1] = src[6];
+    dest[2] = src[5];
+    dest[3] = src[4];
+    dest[4] = src[3];
+    dest[5] = src[2];
+    dest[6] = src[1];
+    dest[7] = src[0];
+#else
+	unsigned char* dest = (unsigned char*) &destVal;
+	unsigned char* src  = (unsigned char*) &sourceVal;
+	dest[0] = src[3];
+    dest[1] = src[2];
+    dest[2] = src[1];
+    dest[3] = src[0];
+#endif //BT_USE_DOUBLE_PRECISION
+}
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void	btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
+{
+	for (int i=0;i<4;i++)
+	{
+		btSwapScalarEndian(sourceVec[i],destVec[i]);
+	}
+
+}
+
+///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void	btUnSwapVector3Endian(btVector3& vector)
+{
+
+	btVector3	swappedVec;
+	for (int i=0;i<4;i++)
+	{
+		btSwapScalarEndian(vector[i],swappedVec[i]);
+	}
+	vector = swappedVec;
+}
+
+#endif //SIMD__VECTOR3_H
Index: code/branches/physics/src/bullet/NEWS
===================================================================
--- code/branches/physics/src/bullet/NEWS	(revision 1963)
+++ code/branches/physics/src/bullet/NEWS	(revision 1963)
@@ -0,0 +1,4 @@
+
+For news, visit the Bullet Physics Forum at
+http://www.continuousphysics.com/Bullet/phpBB2/viewforum.php?f=9
+
Index: code/branches/physics/src/bullet/README
===================================================================
--- code/branches/physics/src/bullet/README	(revision 1963)
+++ code/branches/physics/src/bullet/README	(revision 1963)
@@ -0,0 +1,7 @@
+
+Bullet is a 3D Collision Detection and Rigid Body Dynamics Library for games and animation.
+Free for commercial use, including Playstation 3, open source under the ZLib License.
+Discrete and continuous collision detection, integrated into Blender 3D, and COLLADA 1.4 Physics import. 
+
+See the Bullet_User_Manual.pdf for more info and visit the Bullet Physics Forum at
+http://bulletphysics.com
Index: code/branches/physics/src/bullet/VERSION
===================================================================
--- code/branches/physics/src/bullet/VERSION	(revision 1963)
+++ code/branches/physics/src/bullet/VERSION	(revision 1963)
@@ -0,0 +1,2 @@
+Bullet Collision Detection and Physics Library version 2.72
+http://bullet.googlecode.com
Index: code/branches/physics/src/bullet/btBulletCollisionCommon.h
===================================================================
--- code/branches/physics/src/bullet/btBulletCollisionCommon.h	(revision 1963)
+++ code/branches/physics/src/bullet/btBulletCollisionCommon.h	(revision 1963)
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BULLET_COLLISION_COMMON_H
+#define BULLET_COLLISION_COMMON_H
+
+///Common headerfile includes for Bullet Collision Detection
+
+///Bullet's btCollisionWorld and btCollisionObject definitions
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+///Collision Shapes
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
+#include "BulletCollision/CollisionShapes/btEmptyShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btUniformScalingShape.h"
+
+///Narrowphase Collision Detector
+#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+
+//btSphereBoxCollisionAlgorithm is broken, use gjk for now
+//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+///Dispatching and generation of collision pairs (broadphase)
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
+#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+
+///Math library & Utils
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btDefaultMotionState.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#endif //BULLET_COLLISION_COMMON_H
+
Index: code/branches/physics/src/bullet/btBulletDynamicsCommon.h
===================================================================
--- code/branches/physics/src/bullet/btBulletDynamicsCommon.h	(revision 1963)
+++ code/branches/physics/src/bullet/btBulletDynamicsCommon.h	(revision 1963)
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BULLET_DYNAMICS_COMMON_H
+#define BULLET_DYNAMICS_COMMON_H
+
+///Common headerfile includes for Bullet Dynamics, including Collision Detection
+#include "btBulletCollisionCommon.h"
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Dynamics/btContinuousDynamicsWorld.h"
+
+#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+
+
+///Vehicle simulation, with wheel contact simulated by raycasts
+#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
+
+
+
+
+
+
+#endif //BULLET_DYNAMICS_COMMON_H
+
