Added bullet module.

modules/bullet/bullet.monkey2

@@ -0,0 +1,340 @@
+	
+Namespace bullet
+
+#Import "<libc>"
+#Import "<std>"
+
+#Import "bullet3-2.85.1/src/*.h"
+
+#Import "makefile_linearmath"
+#Import "makefile_collision"
+#Import "makefile_dynamics"
+
+#Import "bullet_glue.cpp"
+#Import "bullet_glue.h"
+
+Using std.geom
+
+Alias btScalar:Float
+
+Extern
+
+Struct btVector3
+	
+	Field x:btScalar="m_floats[0]"
+	Field y:btScalar="m_floats[1]"
+	Field z:btScalar="m_floats[2]"
+	
+	Method New()
+	
+	Method New( x:btScalar,y:btScalar,z:btScalar )
+	
+End
+
+Struct btVector4
+	
+	Field x:btScalar="m_floats[0]"
+	Field y:btScalar="m_floats[1]"
+	Field z:btScalar="m_floats[2]"
+	Field w:btScalar="m_floats[3]"
+	
+	Method New()
+		
+	Method New( x:btScalar,y:btScalar,z:btScalar,w:btScalar )
+	
+End
+
+Struct btMatrix3x3
+
+	Method New()
+		
+	Method New( xx:btScalar,xy:btScalar,xz:btScalar,yx:btScalar,yy:btScalar,yz:btScalar,zx:btScalar,zy:btScalar,zz:btScalar )
+	
+	Method getRow:btVector3( i:Int )
+	
+End
+
+Struct btQuaternion
+
+	Field x:btScalar="m_floats[0]"
+	Field y:btScalar="m_floats[1]"
+	Field z:btScalar="m_floats[2]"
+	Field w:btScalar="m_floats[3]"
+
+	Method New()
+		
+	Method New( x:btScalar,y:btScalar,z:btScalar,w:btScalar )	
+End
+
+Struct btTransform
+	
+	Method New()
+		
+	Method New( q:btQuaternion,c:btVector3=New btVector3( 0,0,0 ) )
+
+	Method New( b:btMatrix3x3,c:btVector3=New btVector3( 0,0,0 ) )
+		
+	Method setOrigin( origin:btVector3 )
+	
+	Method getOrigin:btVector3()
+	
+	Method setBasis( basis:btMatrix3x3 )
+	
+	Method getBasis:btMatrix3x3()
+	
+	Method setFromOpenGLMatrix( m:Float Ptr )
+		
+	Method getOpenGLMatrix( m:Float Ptr )
+		
+	Method getOpenGLMatrix2( m:Float Ptr )
+		
+	Function getIdentity:btTransform()
+End
+
+
+Class btObject Extends Void
+	
+	Method destroy() Extension="delete"
+End
+
+Class btBroadphaseInterface Extends btObject
+End
+
+Class btDbvtBroadphase Extends btBroadphaseInterface
+End
+
+Class btMultiSapBroadphase Extends btBroadphaseInterface
+End
+
+Class btSimpleBroadphase Extends btBroadphaseInterface
+End
+
+Class btCollisionConfiguration Extends btObject
+End
+
+Class btDefaultCollisionConfiguration Extends btCollisionConfiguration
+End
+
+Class btConstraintSolver Extends btObject
+End
+
+Class btSequentialImpulseConstraintSolver Extends btConstraintSolver
+End
+
+Class btDispatcher Extends btObject
+End
+
+Class btCollisionDispatcher Extends btDispatcher
+	
+	Method New( collisionConfiguration:btCollisionConfiguration )
+End
+
+Class btCollisionWorld Extends btObject
+End
+
+Class btDynamicsWorld Extends btCollisionWorld
+	
+	Method setGravity( gravity:btVector3 )
+	
+	Method getGravity:btVector3()
+		
+	Method stepSimulation( timeStep:btScalar,maxSubSteps:Int=1,fixedTimeStep:btScalar=1.0/60.0 ) Virtual
+		
+	Method addRigidBody( body:btRigidBody ) Virtual
+
+	Method addRigidBody( body:btRigidBody,group:Short,mask:Short ) Virtual
+		
+	Method removeRigidBody( body:btRigidBody ) Virtual
+		
+End
+
+Class btDiscreteDynamicsWorld Extends btDynamicsWorld
+	
+	Method New( dispatcher:btDispatcher,pairCache:btBroadphaseInterface,contraintSolver:btConstraintSolver,collisionConfiguration:btCollisionConfiguration )
+		
+End
+
+Class btCollisionShape Extends btObject
+
+	Method setLocalScaling( scaling:btVector3 )
+	
+	Method getLocalScaling:btVector3()
+	
+	Method calculateLocalInertia:btVector3( mass:btScalar ) Extension="bbBullet::calculateLocalInertia"
+End
+
+Class btCompoundShape Extends btCollisionShape
+
+ 	Method New( enableDynamicAabbTree:Bool=True,initialChildCapacity:int=0 )
+ 	
+	Method addChildShape( localTransform:btTransform,shape:btCollisionShape )
+
+End
+
+Class btConcaveShape Extends btCollisionShape
+End
+
+Class btConvexShape Extends btCollisionShape
+End
+
+Class btConvexInternalShape Extends btConvexShape
+End
+
+Class btPolyhedralConvexShape Extends btConvexInternalShape
+End
+
+Class btBoxShape Extends btPolyhedralConvexShape
+	
+	Method New( boxHalfExtents:btVector3 )
+End	
+
+Class btStaticPlaneShape Extends btConcaveShape
+	
+	Method New( planeNormal:btVector3,planeConstant:btScalar )
+End
+
+Class btSphereShape Extends btConvexShape
+
+	Method New( radius:btScalar )
+End
+
+Enum PHY_ScalarType
+End
+
+Const PHY_FLOAT:PHY_ScalarType
+Const PHY_DOUBLE:PHY_ScalarType
+Const PHY_INTEGER:PHY_ScalarType
+Const PHY_SHORT:PHY_ScalarType
+Const PHY_FIXEPOINT88:PHY_ScalarType
+Const PHY_UCHAR:PHY_ScalarType
+
+Class btHeightfieldTerrainShape Extends btConcaveShape
+
+	Method New( heightStickWidth:Int,heightStickLength:Int,heightfieldData:Void Ptr,heightScale:btScalar,minHeight:btScalar,maxHeight:btScalar,upAxis:Int,heightDataType:PHY_ScalarType,flipQuadEdges:Bool )
+
+	Method setUseDiamondSubdivision( useDiamondSubdivision:Bool=True )	
+
+	Method setUseZigZagSubdivision( useZigZagSubdivision:Bool=True )	
+End
+
+Class btTriangleMeshShape Extends btConcaveShape
+
+End
+
+Class btBvhTriangleMeshShape Extends btTriangleMeshShape
+
+	Method New( meshInterface:btStridingMeshInterface,useQuantizedAabbCompression:Bool,buildBvh:Bool=True )
+
+End
+
+Class btStridingMeshInterface Extends btObject
+
+End
+
+Class btTriangleIndexVertexArray Extends btStridingMeshInterface
+
+	Method New( numTriangles:Int,triangleIndexBase:Int Ptr,triangleIndexStride:Int,numVertices:Int,vertexBase:btScalar Ptr,vertexStride:Int )
+	
+
+End
+
+Class btMotionState Extends btObject
+	
+	Method setWorldTransform( worldTrans:btTransform )
+		
+	Method getWorldTransform:btTransform() Extension="bbBullet::getWorldTransform"
+	
+End
+
+Class btDefaultMotionState Extends btMotionState
+
+	Field m_graphicsWorldTrans:btTransform
+ 
+	Field m_centerOfMassOffset:btTransform
+ 
+	Field m_startWorldTrans:btTransform
+	
+	Field m_userPointer:Void Ptr
+	
+'	Method New()
+	
+ 	Method New( startTrans:btTransform=btTransform.getIdentity(),centerOfMassOffset:btTransform=btTransform.getIdentity() )
+
+End
+
+Const CF_STATIC_OBJECT:Int
+Const CF_KINEMATIC_OBJECT:Int
+Const CF_NO_CONTACT_RESPONSE:Int
+Const CF_CUSTOM_MATERIAL_CALLBACK:Int
+Const CF_CHARACTER_OBJECT:Int
+Const CF_DISABLE_VISUALIZE_OBJECT:Int
+Const CF_DISABLE_SPU_COLLISION_PROCESSING:Int
+ 	
+Class btCollisionObject Extends btObject
+
+	Method setWorldTransform( transform:btTransform )
+	
+	Method getWorldTransform:btTransform()
+	
+	Method setRestitution( restitution:btScalar )
+	
+	Method getRestitution:btScalar()
+	
+	Method setFriction( friction:btScalar )
+	
+	Method getFriction:btScalar()
+	
+	Method setRollingFriction( friction:btScalar )
+	
+	Method getRollingFriction:btScalar()
+	
+	Method setCcdSweptSphereRadius( radius:btScalar )
+	
+	Method getCcdSweptSphereRadius:btScalar()
+	
+	Method setCcdMotionThreshold( ccdMotionTheshold:btScalar )
+	
+	Method getCcdMotionThreshold:btScalar()
+	
+	Method setCollisionFlags( flags:Int )
+	
+	Method getCollisionFlags:Int()
+End
+
+Struct btRigidBodyConstructionInfo
+	
+	Field m_mass:btScalar
+	Field m_motionState:btMotionState
+	Field m_startWorldTransform:btTransform
+	Field m_collisionShape:btCollisionShape
+	Field m_localInertia:btVector3
+	Field m_linearDamping:btScalar
+	Field m_angularDamping:btScalar
+	Field m_friction:btScalar
+	Field m_rollingFriction:btScalar
+	Field m_restitution:btScalar
+
+	Field m_linearSleepingThreshold:btScalar
+	Field m_angularSleepingThreshold:btScalar
+	Field m_additionalDamping:Bool
+	Field m_additionalDampingFactor:btScalar
+	Field m_additionalLinearDampingThresholdSqr:btScalar
+	Field m_additionalAngularDampingThresholdSqr:btScalar
+	Field m_additionalAngularDampingFactor:btScalar
+	
+	Method New( mass:btScalar,motionState:btMotionState,collisionShape:btCollisionShape,localInertia:btVector3=New btVector3( 0,0,0 ) )
+End
+	
+Class btRigidBody Extends btCollisionObject
+
+	Method New( constructionInfo:btRigidBodyConstructionInfo )
+
+	Method New( mass:btScalar,motionState:btMotionState,collisionShape:btCollisionShape,localInertia:btVector3=New btVector3( 0,0,0 ) )
+	
+	Method setLinearVelocity( lin_vel:btVector3 )
+	
+	Method getLinearVelocity:btVector3()
+End
+
+Function BulletKludge1( obj:btCollisionObject )="bbBullet::bulletKludge1"
+

modules/bullet/bullet3-2.85.1/AUTHORS.txt

@@ -0,0 +1,40 @@
+Bullet Physics is created by Erwin Coumans with contributions from the following authors / copyright holders:
+
+AMD
+Apple
+Steve Baker
+Gino van den Bergen
+Nicola Candussi
+Erin Catto
+Lawrence Chai
+Erwin Coumans
+Christer Ericson
+Disney Animation
+Google
+Dirk Gregorius
+Marcus Hennix
+MBSim Development Team
+Takahiro Harada
+Simon Hobbs
+John Hsu
+Ole Kniemeyer
+Jay Lee
+Francisco Leon
+Vsevolod Klementjev
+Phil Knight
+John McCutchan
+Steven Peters
+Roman Ponomarev
+Nathanael Presson
+Gabor PUHR
+Arthur Shek
+Russel Smith
+Sony
+Jakub Stephien
+Marten Svanfeldt
+Pierre Terdiman
+Steven Thompson
+Tamas Umenhoffer
+Yunfei Bai
+
+If your name is missing, please send an email to [email protected] or file an issue at http://github.com/bulletphysics/bullet3

modules/bullet/bullet3-2.85.1/LICENSE.txt

@@ -0,0 +1,15 @@
+
+The files in this repository are licensed under the zlib license, except for the files under 'Extras' and examples/ThirdPartyLibs.
+
+Bullet Continuous Collision Detection and Physics Library
+http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.

modules/bullet/bullet3-2.85.1/README.md

@@ -0,0 +1,106 @@
+
+[![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3)
+[![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3)
+
+# Bullet 2.x including pybullet, Virtual Reality support
+
+This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com
+It includes the optional experimental Bullet 3 GPU pipeline.
+
+The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
+The steps towards a new API is in a nutshell:
+
+1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
+2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
+3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
+4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
+5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
+
+You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
+
+## Requirements for Bullet 2
+
+A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,
+but should likely work on any platform with C++ compiler. 
+Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.
+
+## Contributors and Coding Style information
+
+https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
+
+## Requirements for Bullet 3
+
+The entire collision detection and rigid body dynamics is executed on the GPU.
+
+A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
+We succesfully tested the software under Windows, Linux and Mac OSX.
+The software currently doesn't work on OpenCL CPU devices. It might run
+on a laptop GPU but performance will not likely be very good. Note that
+often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
+track down the issue, but more work is required to cover all OpenCL kernels.
+
+## License
+
+All source code files are licensed under the permissive zlib license
+(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
+
+## Build instructions for Bullet using premake. You can also use cmake instead.
+
+**Windows**
+
+Click on build_visual_studio.bat and open build3/vs2010/0MySolution.sln
+
+**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
+
+Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
+Edit this batch file to choose where Python include/lib directories are located.
+Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
+You can connect from Python pybullet to the sandbox using:
+
+```
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+```
+
+**Linux and Mac OSX gnu make**
+
+In a terminal type:
+
+	cd build3
+
+Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
+
+	./premake4_linux gmake
+	./premake4_linux64 gmake
+	./premake4_osx gmake
+
+Then
+
+	cd gmake
+	make
+
+**Mac OSX Xcode**
+	
+Click on build3/xcode4.command or in a terminal window execute
+	
+	./premake_osx xcode4
+
+## Usage
+
+The App_ExampleBrowser executables will be located in the bin folder.
+You can just run it though a terminal/command prompt, or by clicking it.
+
+
+```
+[--start_demo_name="Demo Name"]     Start with a selected demo  
+[--mp4=moviename.mp4]               Create a mp4 movie of the window, requires ffmpeg installed
+[--mouse_move_multiplier=0.400000]  Set the mouse move sensitivity
+[--mouse_wheel_multiplier=0.01]     Set the mouse wheel sensitivity
+[--background_color_red= 0.9]       Set the red component for background color. Same for green and blue
+[--fixed_timestep= 0.0]             Use either a real-time delta time (0.0) or a fixed step size (0.016666)
+```
+
+You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
+Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
+
+See docs folder for further information.

modules/bullet/bullet3-2.85.1/VERSION

@@ -0,0 +1 @@
+2.85

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp

@@ -0,0 +1,37 @@
+
+//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"
+
+
+btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& 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 btVector3& worldAabbMin,const btVector3& 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);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -0,0 +1,1051 @@
+//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 BT_AXIS_SWEEP_3_H
+#define BT_AXIS_SWEEP_3_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:
+	
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	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:
+	btVector3 m_worldAabbMin;						// overall system bounds
+	btVector3 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;
+	btOverlappingPairCache*	m_nullPairCache;
+
+
+	// 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 btVector3& worldAabbMin,const btVector3& 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 btVector3& aabbMin,const btVector3& 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 btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
+
+	virtual void resetPool(btDispatcher* dispatcher);
+
+	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, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+
+	
+	void quantize(BP_FP_INT_TYPE* out, const btVector3& 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)
+		btAssert(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,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
+	} 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>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+{
+	if (m_raycastAccelerator)
+	{
+		m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback);
+	} 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())
+			{
+				Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
+				if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax))
+				{
+					callback.process(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 btVector3& worldAabbMin,const btVector3& 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_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache();
+		m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache);
+		m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
+	}
+
+	//btAssert(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)
+	{
+		m_nullPairCache->~btOverlappingPairCache();
+		btAlignedFree(m_nullPairCache);
+		m_raycastAccelerator->~btDbvtBroadphase();
+		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 btVector3& 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
+	btVector3 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()
+{
+	btAssert(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)
+{
+	btAssert(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 btVector3& aabbMin,const btVector3& 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);
+
+	
+}
+
+template <typename BP_FP_INT_TYPE>
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/)
+{
+	if (m_numHandles == 0)
+	{
+		m_firstFreeHandle = 1;
+		{
+			for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
+				m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
+			m_pHandles[m_maxHandles - 1].SetNextFree(0);
+		}
+	}
+}       
+
+
+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)
+			{
+				///important to use an AABB test that is consistent with the broadphase
+				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 btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
+{
+//	btAssert(bounds.IsFinite());
+	//btAssert(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 btVector3& worldAabbMin,const btVector3& 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 btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
+
+};
+
+#endif
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h

@@ -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_BROADPHASE_INTERFACE_H
+#define 	BT_BROADPHASE_INTERFACE_H
+
+
+
+struct btDispatcherInfo;
+class btDispatcher;
+#include "btBroadphaseProxy.h"
+
+class btOverlappingPairCache;
+
+
+
+struct	btBroadphaseAabbCallback
+{
+	virtual ~btBroadphaseAabbCallback() {}
+	virtual bool	process(const btBroadphaseProxy* proxy) = 0;
+};
+
+
+struct	btBroadphaseRayCallback : public btBroadphaseAabbCallback
+{
+	///added some cached data to accelerate ray-AABB tests
+	btVector3		m_rayDirectionInverse;
+	unsigned int	m_signs[3];
+	btScalar		m_lambda_max;
+
+	virtual ~btBroadphaseRayCallback() {}
+};
+
+#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, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0;
+
+	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) = 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;
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; };
+
+	virtual void	printStats() = 0;
+
+};
+
+#endif //BT_BROADPHASE_INTERFACE_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp

@@ -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"
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h

@@ -0,0 +1,270 @@
+/*
+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_BROADPHASE_PROXY_H
+#define BT_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
+/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code
+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,
+	CUSTOM_POLYHEDRAL_SHAPE_TYPE,
+//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,
+	BOX_2D_SHAPE_PROXYTYPE,
+	CONVEX_2D_SHAPE_PROXYTYPE,
+	CUSTOM_CONVEX_SHAPE_TYPE,
+//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,
+	CUSTOM_CONCAVE_SHAPE_TYPE,
+CONCAVE_SHAPES_END_HERE,
+
+	COMPOUND_SHAPE_PROXYTYPE,
+
+	SOFTBODY_SHAPE_PROXYTYPE,
+	HFFLUID_SHAPE_PROXYTYPE,
+	HFFLUID_BUOYANT_CONVEX_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,
+			CharacterFilter = 32,
+	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+	};
+
+	//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.
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax;
+
+	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_clientObject(userPtr),
+		m_collisionFilterGroup(collisionFilterGroup),
+		m_collisionFilterMask(collisionFilterMask),
+		m_aabbMin(aabbMin),
+		m_aabbMax(aabbMax)
+	{
+		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	isNonMoving(int proxyType)
+	{
+		return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE));
+	}
+
+	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	isSoftBody(int proxyType)
+	{
+		return (proxyType == SOFTBODY_SHAPE_PROXYTYPE);
+	}
+
+	static SIMD_FORCE_INLINE bool isInfinite(int proxyType)
+	{
+		return (proxyType == STATIC_PLANE_PROXYTYPE);
+	}
+
+	static SIMD_FORCE_INLINE bool isConvex2d(int proxyType)
+	{
+		return (proxyType == BOX_2D_SHAPE_PROXYTYPE) ||	(proxyType == CONVEX_2D_SHAPE_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_internalInfo1(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_internalInfo1(other.m_internalInfo1)
+	{
+	}
+	btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
+	{
+
+		//keep them sorted, so the std::set operations work
+		if (proxy0.m_uniqueId < proxy1.m_uniqueId)
+        { 
+            m_pProxy0 = &proxy0; 
+            m_pProxy1 = &proxy1; 
+        }
+        else 
+        { 
+			m_pProxy0 = &proxy1; 
+            m_pProxy1 = &proxy0; 
+        }
+
+		m_algorithm = 0;
+		m_internalInfo1 = 0;
+
+	}
+	
+	btBroadphaseProxy* m_pProxy0;
+	btBroadphaseProxy* m_pProxy1;
+	
+	mutable btCollisionAlgorithm* m_algorithm;
+	union { void* m_internalInfo1; int m_internalTmpValue;};//don't use this data, it will be removed in future version.
+
+};
+
+/*
+//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 ) const
+		{
+			const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
+			const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
+			const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
+			const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
+
+			 return uidA0 > uidB0 || 
+				(a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
+				(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 //BT_BROADPHASE_PROXY_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp

@@ -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;
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h

@@ -0,0 +1,81 @@
+/*
+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_ALGORITHM_H
+#define BT_COLLISION_ALGORITHM_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct btBroadphaseProxy;
+class btDispatcher;
+class btManifoldResult;
+class btCollisionObject;
+struct btCollisionObjectWrapper;
+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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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 //BT_COLLISION_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -0,0 +1,1297 @@
+/*
+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)
+	ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]);
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	res=*ptr;
+#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[1],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(btDot(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
+	ATTRIBUTE_ALIGNED16(char	locals[sizeof(btDbvtVolume)]);
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	volume=*ptr;
+	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][btDot(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);
+}
+
+#if 0
+static DBVT_INLINE btDbvtNode*	walkup(btDbvtNode* n,int count)
+{
+	while(n&&(count--)) n=n->parent;
+	return(n);
+}
+#endif
+
+//
+// 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;
+	m_lkhd		=	-1;
+	m_stkStack.clear();
+	m_opath		=	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,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

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -0,0 +1,1332 @@
+/*
+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
+
+// Specific methods implementation
+
+//SSE gives errors on a MSVC 7.1
+#if defined (BT_USE_SSE) //&& defined (_WIN32)
+#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*)1;(void)typechecker;
+#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
+#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
+#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 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);
+    
+    DBVT_INLINE btVector3&	tMins()	{ return(mi); }
+	DBVT_INLINE btVector3&	tMaxs()	{ return(mx); }
+    
+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;
+	};
+};
+
+typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
+
+
+///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;
+
+	
+	btAlignedObjectArray<sStkNN>	m_stkStack;
+	mutable btAlignedObjectArray<const btDbvtNode*>	m_rayTestStack;
+
+
+	// 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,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
+		void		collideTT(	const btDbvtNode* root0,
+		const btDbvtNode* root1,
+		DBVT_IPOLICY);
+
+	DBVT_PREFIX
+		void		collideTTpersistentStack(	const btDbvtNode* root0,
+		  const btDbvtNode* root1,
+		  DBVT_IPOLICY);
+#if 0
+	DBVT_PREFIX
+		void		collideTT(	const btDbvtNode* root0,
+		const btDbvtNode* root1,
+		const btTransform& xform,
+		DBVT_IPOLICY);
+	DBVT_PREFIX
+		void		collideTT(	const btDbvtNode* root0,
+		const btTransform& xform0,
+		const btDbvtNode* root1,
+		const btTransform& xform1,
+		DBVT_IPOLICY);
+#endif
+
+	DBVT_PREFIX
+		void		collideTV(	const btDbvtNode* root,
+		const btDbvtVolume& volume,
+		DBVT_IPOLICY) const;
+	
+	DBVT_PREFIX
+	void		collideTVNoStackAlloc(	const btDbvtNode* root,
+						  const btDbvtVolume& volume,
+						  btNodeStack& stack,
+						  DBVT_IPOLICY) const;
+	
+	
+	
+	
+	///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
+	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
+	DBVT_PREFIX
+		static void		rayTest(	const btDbvtNode* root,
+		const btVector3& rayFrom,
+		const btVector3& rayTo,
+		DBVT_IPOLICY);
+	///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
+	///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
+	DBVT_PREFIX
+		void		rayTestInternal(	const btDbvtNode* root,
+								const btVector3& rayFrom,
+								const btVector3& rayTo,
+								const btVector3& rayDirectionInverse,
+								unsigned int signs[3],
+								btScalar lambda_max,
+								const btVector3& aabbMin,
+								const btVector3& aabbMax,
+								DBVT_IPOLICY) const;
+
+	DBVT_PREFIX
+		static void		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((btDot(n,px)+o)<0)		return(-1);
+	if((btDot(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(btDot(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))));
+#if defined (_WIN32)
+	const __int32*	pu((const __int32*)&rt);
+#else
+    const int*	pu((const int*)&rt);
+#endif
+	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 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
+    
+#if defined (_WIN32)
+	static ATTRIBUTE_ALIGNED16(const unsigned __int32)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+#else
+    static ATTRIBUTE_ALIGNED16(const unsigned int)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/};
+#endif
+	///@todo: the intrinsic version is 11% slower
+#if DBVT_USE_INTRINSIC_SSE
+
+	union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
+	{
+	   __m128		ssereg;
+	   float		floats[4];
+	   int			ints[4];
+	};
+
+	__m128	omi(_mm_load_ps(o.mi));
+	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
+	__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));
+	
+	btSSEUnion tmp;
+	tmp.ssereg = _mm_cmple_ss(bmi,ami);
+	return tmp.ints[0]&1;
+
+#else
+	ATTRIBUTE_ALIGNED16(__int32	r[1]);
+	__asm
+	{
+		mov		eax,o
+			mov		ecx,a
+			mov		edx,b
+			movaps	xmm0,[eax]
+		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)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=stkStack[--depth];
+				if(depth>treshold)
+				{
+					stkStack.resize(stkStack.size()*2);
+					treshold=stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+
+
+DBVT_PREFIX
+inline void		btDbvt::collideTTpersistentStack(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			
+			m_stkStack.resize(DOUBLE_STACKSIZE);
+			m_stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=m_stkStack[--depth];
+				if(depth>treshold)
+				{
+					m_stkStack.resize(m_stkStack.size()*2);
+					treshold=m_stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+#if 0
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTT(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform,
+								  DBVT_IPOLICY)
+{
+	DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{
+				sStkNN	p=stkStack[--depth];
+				if(Intersect(p.a->volume,p.b->volume,xform))
+				{
+					if(depth>treshold)
+					{
+						stkStack.resize(stkStack.size()*2);
+						treshold=stkStack.size()-4;
+					}
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{					
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTT(	const btDbvtNode* root0,
+								  const btTransform& xform0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform1,
+								  DBVT_IPOLICY)
+{
+	const btTransform	xform=xform0.inverse()*xform1;
+	collideTT(root0,root1,xform,policy);
+}
+#endif 
+
+DBVT_PREFIX
+inline void		btDbvt::collideTV(	const btDbvtNode* root,
+								  const btDbvtVolume& vol,
+								  DBVT_IPOLICY) const
+{
+	DBVT_CHECKTYPE
+	if(root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
+		btAlignedObjectArray<const btDbvtNode*>	stack;
+		stack.resize(0);
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do	{
+			const btDbvtNode*	n=stack[stack.size()-1];
+			stack.pop_back();
+			if(Intersect(n->volume,volume))
+			{
+				if(n->isinternal())
+				{
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
+				}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while(stack.size()>0);
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTVNoStackAlloc(	const btDbvtNode* root,
+											 const btDbvtVolume& vol,
+											 btNodeStack& stack,
+											 DBVT_IPOLICY) const
+{
+	DBVT_CHECKTYPE
+	if(root)
+	{
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)		volume(vol);
+		stack.resize(0);
+		stack.reserve(SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do	{
+			const btDbvtNode*	n=stack[stack.size()-1];
+			stack.pop_back();
+			if(Intersect(n->volume,volume))
+			{
+				if(n->isinternal())
+				{
+					stack.push_back(n->childs[0]);
+					stack.push_back(n->childs[1]);
+				}
+				else
+				{
+					policy.Process(n);
+				}
+			}
+		} while(stack.size()>0);
+	}
+}
+
+
+DBVT_PREFIX
+inline void		btDbvt::rayTestInternal(	const btDbvtNode* root,
+								const btVector3& rayFrom,
+								const btVector3& rayTo,
+								const btVector3& rayDirectionInverse,
+								unsigned int signs[3],
+								btScalar lambda_max,
+								const btVector3& aabbMin,
+								const btVector3& aabbMax,
+								DBVT_IPOLICY) const
+{
+        (void) rayTo;
+	DBVT_CHECKTYPE
+	if(root)
+	{
+		btVector3 resultNormal;
+
+		int								depth=1;
+		int								treshold=DOUBLE_STACKSIZE-2;
+		btAlignedObjectArray<const btDbvtNode*>&	stack = m_rayTestStack;
+		stack.resize(DOUBLE_STACKSIZE);
+		stack[0]=root;
+		btVector3 bounds[2];
+		do	
+		{
+			const btDbvtNode*	node=stack[--depth];
+			bounds[0] = node->volume.Mins()-aabbMax;
+			bounds[1] = node->volume.Maxs()-aabbMin;
+			btScalar tmin=1.f,lambda_min=0.f;
+			unsigned int result1=false;
+			result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+			if(result1)
+			{
+				if(node->isinternal())
+				{
+					if(depth>treshold)
+					{
+						stack.resize(stack.size()*2);
+						treshold=stack.size()-2;
+					}
+					stack[depth++]=node->childs[0];
+					stack[depth++]=node->childs[1];
+				}
+				else
+				{
+					policy.Process(node);
+				}
+			}
+		} while(depth);
+	}
+}
+
+//
+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/BT_LARGE_FLOAT
+			btVector3 rayDirectionInverse;
+			rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+			rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+			rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
+			unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+
+			btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
+
+			btVector3 resultNormal;
+
+			btAlignedObjectArray<const btDbvtNode*>	stack;
+
+			int								depth=1;
+			int								treshold=DOUBLE_STACKSIZE-2;
+
+			stack.resize(DOUBLE_STACKSIZE);
+			stack[0]=root;
+			btVector3 bounds[2];
+			do	{
+				const btDbvtNode*	node=stack[--depth];
+
+				bounds[0] = node->volume.Mins();
+				bounds[1] = node->volume.Maxs();
+				
+				btScalar tmin=1.f,lambda_min=0.f;
+				unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+
+#ifdef COMPARE_BTRAY_AABB2
+				btScalar param=1.f;
+				bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
+				btAssert(result1 == result2);
+#endif //TEST_BTRAY_AABB2
+
+				if(result1)
+				{
+					if(node->isinternal())
+					{
+						if(depth>treshold)
+						{
+							stack.resize(stack.size()*2);
+							treshold=stack.size()-2;
+						}
+						stack[depth++]=node->childs[0];
+						stack[depth++]=node->childs[1];
+					}
+					else
+					{
+						policy.Process(node);
+					}
+				}
+			} while(depth);
+
+		}
+}
+
+//
+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);
+							
+							//void * memmove ( void * destination, const void * source, size_t num );
+							
+#if DBVT_USE_MEMMOVE
+                     {
+                     int num_items_to_move = stack.size()-1-j;
+                     if(num_items_to_move > 0)
+                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
+                     }
+#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
+                     {
+                     int num_items_to_move = stack.size()-1-j;
+                     if(num_items_to_move > 0)
+                        memmove(&stack[j+1],&stack[j],sizeof(int)*num_items_to_move);
+                     }
+#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

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -0,0 +1,796 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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->m_uniqueId>pb->m_uniqueId) 
+				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		=	0;
+	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;
+		m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
+		m_sets[1].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;
+}
+
+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);
+	}
+};	
+
+void	btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	BroadphaseRayTester callback(rayCallback);
+
+	m_sets[0].rayTestInternal(	m_sets[0].m_root,
+		rayFrom,
+		rayTo,
+		rayCallback.m_rayDirectionInverse,
+		rayCallback.m_signs,
+		rayCallback.m_lambda_max,
+		aabbMin,
+		aabbMax,
+		callback);
+
+	m_sets[1].rayTestInternal(	m_sets[1].m_root,
+		rayFrom,
+		rayTo,
+		rayCallback.m_rayDirectionInverse,
+		rayCallback.m_signs,
+		rayCallback.m_lambda_max,
+		aabbMin,
+		aabbMax,
+		callback);
+
+}
+
+
+struct	BroadphaseAabbTester : btDbvt::ICollide
+{
+	btBroadphaseAabbCallback& m_aabbCallback;
+	BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
+		:m_aabbCallback(orgCallback)
+	{
+	}
+	void					Process(const btDbvtNode* leaf)
+	{
+		btDbvtProxy*	proxy=(btDbvtProxy*)leaf->data;
+		m_aabbCallback.process(proxy);
+	}
+};	
+
+void	btDbvtBroadphase::aabbTest(const btVector3& aabbMin,const btVector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
+{
+	BroadphaseAabbTester callback(aabbCallback);
+
+	const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
+		//process all children, that overlap with  the given AABB bounds
+	m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
+	m_sets[1].collideTV(m_sets[1].m_root,bounds,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);
+				m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
+				m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+			}
+		}	
+	}
+}
+
+
+//
+void							btDbvtBroadphase::setAabbForceUpdate(		btBroadphaseProxy* absproxy,
+														  const btVector3& aabbMin,
+														  const btVector3& aabbMax,
+														  btDispatcher* /*dispatcher*/)
+{
+	btDbvtProxy*						proxy=(btDbvtProxy*)absproxy;
+	ATTRIBUTE_ALIGNED16(btDbvtVolume)	aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
+	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;
+		/* 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);
+			m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
+			m_sets[0].collideTTpersistentStack(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
+
+	performDeferredRemoval(dispatcher);
+
+}
+
+void btDbvtBroadphase::performDeferredRemoval(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());
+
+		int 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)
+			{
+				//important to perform AABB check that is consistent with the broadphase
+				btDbvtProxy*		pa=(btDbvtProxy*)pair.m_pProxy0;
+				btDbvtProxy*		pb=(btDbvtProxy*)pair.m_pProxy1;
+				bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+				btAssert(!pair.m_algorithm);
+			}
+			
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair,dispatcher);
+
+				pair.m_pProxy0 = 0;
+				pair.m_pProxy1 = 0;
+				invalidPair++;
+			} 
+			
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
+}
+
+//
+void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
+{
+	/*printf("---------------------------------------------------------\n");
+	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+	{
+		int i;
+		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+		{
+			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+		}
+		printf("\n");
+	}
+*/
+
+
+
+	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);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
+		}
+		if(m_deferedcollide)
+		{
+			SPC(m_profiling.m_ddcollide);
+			m_sets[0].collideTTpersistentStack(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)
+		{
+
+			int			ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
+			for(int i=0;i<ni;++i)
+			{
+				btBroadphasePair&	p=pairs[(m_cid+i)%pairs.size()];
+				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->m_uniqueId>pb->m_uniqueId) 
+						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::resetPool(btDispatcher* dispatcher)
+{
+	
+	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
+	if (!totalObjects)
+	{
+		//reset internal dynamic tree data structures
+		m_sets[0].clear();
+		m_sets[1].clear();
+		
+		m_deferedcollide	=	false;
+		m_needcleanup		=	true;
+		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_gid				=	0;
+		m_pid				=	0;
+		m_cid				=	0;
+		for(int i=0;i<=STAGECOUNT;++i)
+		{
+			m_stageRoots[i]=0;
+		}
+	}
+}
+
+//
+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
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -0,0 +1,146 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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);
+	virtual void					destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
+	virtual void					setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
+	virtual void					rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
+	virtual void					aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+
+	virtual void					getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
+	virtual	void					calculateOverlappingPairs(btDispatcher* dispatcher);
+	virtual	btOverlappingPairCache*	getOverlappingPairCache();
+	virtual	const btOverlappingPairCache*	getOverlappingPairCache() const;
+	virtual	void					getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
+	virtual	void					printStats();
+
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
+	void	performDeferredRemoval(btDispatcher* dispatcher);
+	
+	void	setVelocityPrediction(btScalar prediction)
+	{
+		m_prediction = prediction;
+	}
+	btScalar getVelocityPrediction() const
+	{
+		return m_prediction;
+	}
+
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. 
+	///it is not part of the btBroadphaseInterface but specific to btDbvtBroadphase.
+	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
+	///http://code.google.com/p/bullet/issues/detail?id=223
+	void							setAabbForceUpdate(		btBroadphaseProxy* absproxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* /*dispatcher*/);
+
+	static void						benchmark(btBroadphaseInterface*);
+
+
+};
+
+#endif

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp

@@ -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()
+{
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -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 BT_DISPATCHER_H
+#define BT_DISPATCHER_H
+#include "LinearMath/btScalar.h"
+
+class btCollisionAlgorithm;
+struct btBroadphaseProxy;
+class btRigidBody;
+class	btCollisionObject;
+class btOverlappingPairCache;
+struct btCollisionObjectWrapper;
+
+class btPersistentManifold;
+class btPoolAllocator;
+
+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(true),
+		m_debugDraw(0),
+		m_enableSatConvex(false),
+		m_enableSPU(true),
+		m_useEpa(true),
+		m_allowedCcdPenetration(btScalar(0.04)),
+		m_useConvexConservativeDistanceUtil(false),
+		m_convexConservativeDistanceThreshold(0.0f)
+	{
+
+	}
+	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;
+	bool		m_useConvexConservativeDistanceUtil;
+	btScalar	m_convexConservativeDistanceThreshold;
+};
+
+///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(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0;
+
+	virtual btPersistentManifold*	getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;
+
+	virtual void releaseManifold(btPersistentManifold* manifold)=0;
+
+	virtual void clearManifold(btPersistentManifold* manifold)=0;
+
+	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) = 0;
+
+	virtual bool	needsResponse(const btCollisionObject* body0,const 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	btPoolAllocator*	getInternalManifoldPool() = 0;
+
+	virtual	const btPoolAllocator*	getInternalManifoldPool() const = 0;
+
+	virtual	void* allocateCollisionAlgorithm(int size)  = 0;
+
+	virtual	void freeCollisionAlgorithm(void* ptr) = 0;
+
+};
+
+
+#endif //BT_DISPATCHER_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp

@@ -0,0 +1,489 @@
+/*
+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, const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	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);
+
+
+
+	
+	if (m_optimizedAabbTree)
+		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 ) const
+		{
+				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();
+
+		}
+		*/
+
+}
+
+void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
+{
+	// not yet
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h

@@ -0,0 +1,151 @@
+/*
+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 research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead.
+///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,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
+
+	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(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+		aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+	}
+
+	void	buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
+
+	virtual void	printStats();
+
+	void quicksort (btBroadphasePairArray& a, int lo, int hi);
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
+};
+
+#endif //BT_MULTI_SAP_BROADPHASE

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -0,0 +1,632 @@
+/*
+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 "LinearMath/btAabbUtil2.h"
+
+#include <stdio.h>
+
+int	gOverlappingPairs = 0;
+
+int gRemovePairs =0;
+int gAddedPairs =0;
+int gFindPairs =0;
+
+
+
+
+btHashedOverlappingPairCache::btHashedOverlappingPairCache():
+	m_overlapFilterCallback(0),
+	m_ghostPairCallback(0)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+
+btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
+{
+}
+
+
+
+void	btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
+{
+	if (pair.m_algorithm && dispatcher)
+	{
+		{
+			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->m_uniqueId>proxy1->m_uniqueId) 
+		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->m_uniqueId>proxy1->m_uniqueId) 
+		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.expandNonInitializing();
+
+	//this is where we add an actual pair, so also call the 'ghost'
+	if (m_ghostPairCallback)
+		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
+
+	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_internalTmpValue = 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->m_uniqueId>proxy1->m_uniqueId) 
+		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_internalInfo1;
+
+	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 (m_ghostPairCallback)
+		m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+	// 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>
+#include "LinearMath/btQuickprof.h"
+void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
+{
+	BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
+	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	btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+	///need to keep hashmap in sync with pair address, so rebuild all
+	btBroadphasePairArray tmpPairs;
+	int i;
+	for (i=0;i<m_overlappingPairArray.size();i++)
+	{
+		tmpPairs.push_back(m_overlappingPairArray[i]);
+	}
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
+	}
+	
+	for (i = 0; i < m_next.size(); i++)
+	{
+		m_next[i] = BT_NULL_PAIR;
+	}
+
+	tmpPairs.quickSort(btBroadphasePairSortPredicate());
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
+	}
+
+	
+}
+
+
+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_internalInfo1;
+			cleanOverlappingPair(pair,dispatcher);
+			if (m_ghostPairCallback)
+				m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,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
+	btAssert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+	
+	void* mem = &m_overlappingPairArray.expandNonInitializing();
+	btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
+	
+	gOverlappingPairs++;
+	gAddedPairs++;
+	
+	if (m_ghostPairCallback)
+		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
+	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())
+	{
+		//btAssert(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);
+			pair->m_pProxy0 = 0;
+			pair->m_pProxy1 = 0;
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.pop_back();
+			gOverlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+
+btSortedOverlappingPairCache::btSortedOverlappingPairCache():
+	m_blockedForChanges(false),
+	m_hasDeferredRemoval(true),
+	m_overlapFilterCallback(0),
+	m_ghostPairCallback(0)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
+{
+}
+
+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);
+}
+
+void	btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+	//should already be sorted
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h

@@ -0,0 +1,469 @@
+/*
+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_OVERLAPPING_PAIR_CACHE_H
+#define BT_OVERLAPPING_PAIR_CACHE_H
+
+
+#include "btBroadphaseInterface.h"
+#include "btBroadphaseProxy.h"
+#include "btOverlappingPairCallback.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;
+
+	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
+
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher) = 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;
+
+protected:
+	
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	btOverlappingPairCallback*	m_ghostPairCallback;
+
+
+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;
+	}
+
+	virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+	{
+		m_ghostPairCallback = ghostPairCallback;
+	}
+
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
+	
+
+	
+};
+
+
+
+
+///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;
+
+		btOverlappingPairCallback*	m_ghostPairCallback;
+
+	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;
+		}
+
+		virtual	void	setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
+		{
+			m_ghostPairCallback = ghostPairCallback;
+		}
+
+		virtual void	sortOverlappingPairs(btDispatcher* dispatcher);
+		
+
+};
+
+
+
+///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit 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	void	setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
+	{
+
+	}
+
+	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*/)
+	{
+	}
+	
+	virtual void	sortOverlappingPairs(btDispatcher* dispatcher)
+	{
+        (void) dispatcher;
+	}
+
+
+};
+
+
+#endif //BT_OVERLAPPING_PAIR_CACHE_H
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h

@@ -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

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -0,0 +1,1395 @@
+/*
+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"
+#include "LinearMath/btSerializer.h"
+
+#define RAYAABB2
+
+btQuantizedBvh::btQuantizedBvh() : 
+					m_bulletVersion(BT_BULLET_VERSION),
+					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(1,0,0),
+	btVector3(0,1,0),
+	btVector3(0,0,1),
+	btVector3(0,1,1)
+};
+#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;
+
+	{
+		unsigned short vecIn[3];
+		btVector3 v;
+		{
+			quantize(vecIn,m_bvhAabbMin,false);
+			v = unQuantize(vecIn);
+			m_bvhAabbMin.setMin(v-clampValue);
+		}
+        aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+        m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+		{
+			quantize(vecIn,m_bvhAabbMax,true);
+			v = unQuantize(vecIn);
+			m_bvhAabbMax.setMax(v+clampValue);
+		}
+		aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+		m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
+	}
+}
+
+
+
+
+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 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(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+	rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+	rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : 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] -= aabbMax;
+		bounds[1] -= aabbMin;
+
+		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 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(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
+	rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1];
+	rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : 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] -= aabbMax;
+			bounds[1] -= aabbMin;
+			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>
+
+#if 0
+//PCK: consts
+static const unsigned BVH_ALIGNMENT = 16;
+static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1;
+
+static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
+#endif
+
+
+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() const
+{
+	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) const
+{
+	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),
+m_bulletVersion(BT_BULLET_VERSION)
+{
+
+}
+
+void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData)
+{
+	m_bvhAabbMax.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMax);
+	m_bvhAabbMin.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMin);
+	m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization);
+
+	m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex;
+	m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0;
+	
+	{
+		int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes;
+		m_contiguousNodes.resize(numElem);
+
+		if (numElem)
+		{
+			btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr;
+
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_contiguousNodes[i].m_aabbMaxOrg.deSerializeFloat(memPtr->m_aabbMaxOrg);
+				m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg);
+				m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex;
+				m_contiguousNodes[i].m_subPart = memPtr->m_subPart;
+				m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex;
+			}
+		}
+	}
+
+	{
+		int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes;
+		m_quantizedContiguousNodes.resize(numElem);
+		
+		if (numElem)
+		{
+			btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr;
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+			}
+		}
+	}
+
+	m_traversalMode = btTraversalMode(quantizedBvhFloatData.m_traversalMode);
+	
+	{
+		int numElem = quantizedBvhFloatData.m_numSubtreeHeaders;
+		m_SubtreeHeaders.resize(numElem);
+		if (numElem)
+		{
+			btBvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr;
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+				m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+				m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+				m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex;
+				m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize;
+			}
+		}
+	}
+}
+
+void btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData)
+{
+	m_bvhAabbMax.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMax);
+	m_bvhAabbMin.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMin);
+	m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization);
+
+	m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex;
+	m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0;
+	
+	{
+		int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes;
+		m_contiguousNodes.resize(numElem);
+
+		if (numElem)
+		{
+			btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr;
+
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_contiguousNodes[i].m_aabbMaxOrg.deSerializeDouble(memPtr->m_aabbMaxOrg);
+				m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg);
+				m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex;
+				m_contiguousNodes[i].m_subPart = memPtr->m_subPart;
+				m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex;
+			}
+		}
+	}
+
+	{
+		int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes;
+		m_quantizedContiguousNodes.resize(numElem);
+		
+		if (numElem)
+		{
+			btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr;
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex = memPtr->m_escapeIndexOrTriangleIndex;
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+				m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+			}
+		}
+	}
+
+	m_traversalMode = btTraversalMode(quantizedBvhDoubleData.m_traversalMode);
+	
+	{
+		int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders;
+		m_SubtreeHeaders.resize(numElem);
+		if (numElem)
+		{
+			btBvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr;
+			for (int i=0;i<numElem;i++,memPtr++)
+			{
+				m_SubtreeHeaders[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0] ;
+				m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1];
+				m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1];
+				m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2];
+				m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex;
+				m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize;
+			}
+		}
+	}
+
+}
+
+
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btQuantizedBvhData* quantizedData = (btQuantizedBvhData*)dataBuffer;
+	
+	m_bvhAabbMax.serialize(quantizedData->m_bvhAabbMax);
+	m_bvhAabbMin.serialize(quantizedData->m_bvhAabbMin);
+	m_bvhQuantization.serialize(quantizedData->m_bvhQuantization);
+
+	quantizedData->m_curNodeIndex = m_curNodeIndex;
+	quantizedData->m_useQuantization = m_useQuantization;
+	
+	quantizedData->m_numContiguousLeafNodes = m_contiguousNodes.size();
+	quantizedData->m_contiguousNodesPtr = (btOptimizedBvhNodeData*) (m_contiguousNodes.size() ? serializer->getUniquePointer((void*)&m_contiguousNodes[0]) : 0);
+	if (quantizedData->m_contiguousNodesPtr)
+	{
+		int sz = sizeof(btOptimizedBvhNodeData);
+		int numElem = m_contiguousNodes.size();
+		btChunk* chunk = serializer->allocate(sz,numElem);
+		btOptimizedBvhNodeData* memPtr = (btOptimizedBvhNodeData*)chunk->m_oldPtr;
+		for (int i=0;i<numElem;i++,memPtr++)
+		{
+			m_contiguousNodes[i].m_aabbMaxOrg.serialize(memPtr->m_aabbMaxOrg);
+			m_contiguousNodes[i].m_aabbMinOrg.serialize(memPtr->m_aabbMinOrg);
+			memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex;
+			memPtr->m_subPart = m_contiguousNodes[i].m_subPart;
+			memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex;
+		}
+		serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]);
+	}
+
+	quantizedData->m_numQuantizedContiguousNodes = m_quantizedContiguousNodes.size();
+//	printf("quantizedData->m_numQuantizedContiguousNodes=%d\n",quantizedData->m_numQuantizedContiguousNodes);
+	quantizedData->m_quantizedContiguousNodesPtr =(btQuantizedBvhNodeData*) (m_quantizedContiguousNodes.size() ? serializer->getUniquePointer((void*)&m_quantizedContiguousNodes[0]) : 0);
+	if (quantizedData->m_quantizedContiguousNodesPtr)
+	{
+		int sz = sizeof(btQuantizedBvhNodeData);
+		int numElem = m_quantizedContiguousNodes.size();
+		btChunk* chunk = serializer->allocate(sz,numElem);
+		btQuantizedBvhNodeData* memPtr = (btQuantizedBvhNodeData*)chunk->m_oldPtr;
+		for (int i=0;i<numElem;i++,memPtr++)
+		{
+			memPtr->m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[i].m_escapeIndexOrTriangleIndex;
+			memPtr->m_quantizedAabbMax[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[0];
+			memPtr->m_quantizedAabbMax[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[1];
+			memPtr->m_quantizedAabbMax[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[2];
+			memPtr->m_quantizedAabbMin[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[0];
+			memPtr->m_quantizedAabbMin[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[1];
+			memPtr->m_quantizedAabbMin[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[2];
+		}
+		serializer->finalizeChunk(chunk,"btQuantizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_quantizedContiguousNodes[0]);
+	}
+
+	quantizedData->m_traversalMode = int(m_traversalMode);
+	quantizedData->m_numSubtreeHeaders = m_SubtreeHeaders.size();
+
+	quantizedData->m_subTreeInfoPtr = (btBvhSubtreeInfoData*) (m_SubtreeHeaders.size() ? serializer->getUniquePointer((void*)&m_SubtreeHeaders[0]) : 0);
+	if (quantizedData->m_subTreeInfoPtr)
+	{
+		int sz = sizeof(btBvhSubtreeInfoData);
+		int numElem = m_SubtreeHeaders.size();
+		btChunk* chunk = serializer->allocate(sz,numElem);
+		btBvhSubtreeInfoData* memPtr = (btBvhSubtreeInfoData*)chunk->m_oldPtr;
+		for (int i=0;i<numElem;i++,memPtr++)
+		{
+			memPtr->m_quantizedAabbMax[0] = m_SubtreeHeaders[i].m_quantizedAabbMax[0];
+			memPtr->m_quantizedAabbMax[1] = m_SubtreeHeaders[i].m_quantizedAabbMax[1];
+			memPtr->m_quantizedAabbMax[2] = m_SubtreeHeaders[i].m_quantizedAabbMax[2];
+			memPtr->m_quantizedAabbMin[0] = m_SubtreeHeaders[i].m_quantizedAabbMin[0];
+			memPtr->m_quantizedAabbMin[1] = m_SubtreeHeaders[i].m_quantizedAabbMin[1];
+			memPtr->m_quantizedAabbMin[2] = m_SubtreeHeaders[i].m_quantizedAabbMin[2];
+
+			memPtr->m_rootNodeIndex = m_SubtreeHeaders[i].m_rootNodeIndex;
+			memPtr->m_subtreeSize = m_SubtreeHeaders[i].m_subtreeSize;
+		}
+		serializer->finalizeChunk(chunk,"btBvhSubtreeInfoData",BT_ARRAY_CODE,(void*)&m_SubtreeHeaders[0]);
+	}
+	return btQuantizedBvhDataName;
+}
+
+
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h

@@ -0,0 +1,581 @@
+/*
+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_QUANTIZED_BVH_H
+#define BT_QUANTIZED_BVH_H
+
+class btSerializer;
+
+//#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"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btQuantizedBvhData btQuantizedBvhDoubleData
+#define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData
+#define btQuantizedBvhDataName "btQuantizedBvhDoubleData"
+#else
+#define btQuantizedBvhData btQuantizedBvhFloatData
+#define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData
+#define btQuantizedBvhDataName "btQuantizedBvhFloatData"
+#endif
+
+
+
+//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());
+		unsigned int x=0;
+		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
+		// Get only the lower bits where the triangle index is stored
+		return (m_escapeIndexOrTriangleIndex&~(y));
+	}
+	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;
+
+//pad the size to 64 bytes
+	char	m_padding[20];
+};
+
+
+///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
+{
+public:
+	enum btTraversalMode
+	{
+		TRAVERSAL_STACKLESS = 0,
+		TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
+		TRAVERSAL_RECURSIVE
+	};
+
+protected:
+
+
+	btVector3			m_bvhAabbMin;
+	btVector3			m_bvhAabbMax;
+	btVector3			m_bvhQuantization;
+
+	int					m_bulletVersion;	//for serialization versioning. It could also be used to detect endianess.
+
+	int					m_curNodeIndex;
+	//quantization data
+	bool				m_useQuantization;
+
+
+
+	NodeArray			m_leafNodes;
+	NodeArray			m_contiguousNodes;
+	QuantizedNodeArray	m_quantizedLeafNodes;
+	QuantizedNodeArray	m_quantizedContiguousNodes;
+	
+	btTraversalMode	m_traversalMode;
+	BvhSubtreeInfoArray		m_SubtreeHeaders;
+
+	//This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
+	mutable 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;
+	
+
+
+
+	void	updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
+
+public:
+	
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	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() const;
+
+	/// Data buffer MUST be 16 byte aligned
+	virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
+
+	///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();
+//////////////////////////////////////////////////////////////////////
+
+	
+	virtual	int	calculateSerializeBufferSizeNew() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	virtual	void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData);
+
+	virtual	void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData);
+
+
+////////////////////////////////////////////////////////////////////
+
+	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);
+
+}
+;
+
+
+struct	btBvhSubtreeInfoData
+{
+	int			m_rootNodeIndex;
+	int			m_subtreeSize;
+	unsigned short m_quantizedAabbMin[3];
+	unsigned short m_quantizedAabbMax[3];
+};
+
+struct btOptimizedBvhNodeFloatData
+{
+	btVector3FloatData	m_aabbMinOrg;
+	btVector3FloatData	m_aabbMaxOrg;
+	int	m_escapeIndex;
+	int	m_subPart;
+	int	m_triangleIndex;
+	char m_pad[4];
+};
+
+struct btOptimizedBvhNodeDoubleData
+{
+	btVector3DoubleData	m_aabbMinOrg;
+	btVector3DoubleData	m_aabbMaxOrg;
+	int	m_escapeIndex;
+	int	m_subPart;
+	int	m_triangleIndex;
+	char	m_pad[4];
+};
+
+
+struct btQuantizedBvhNodeData
+{
+	unsigned short m_quantizedAabbMin[3];
+	unsigned short m_quantizedAabbMax[3];
+	int	m_escapeIndexOrTriangleIndex;
+};
+
+struct	btQuantizedBvhFloatData
+{
+	btVector3FloatData			m_bvhAabbMin;
+	btVector3FloatData			m_bvhAabbMax;
+	btVector3FloatData			m_bvhQuantization;
+	int					m_curNodeIndex;
+	int					m_useQuantization;
+	int					m_numContiguousLeafNodes;
+	int					m_numQuantizedContiguousNodes;
+	btOptimizedBvhNodeFloatData	*m_contiguousNodesPtr;
+	btQuantizedBvhNodeData		*m_quantizedContiguousNodesPtr;
+	btBvhSubtreeInfoData	*m_subTreeInfoPtr;
+	int					m_traversalMode;
+	int					m_numSubtreeHeaders;
+	
+};
+
+struct	btQuantizedBvhDoubleData
+{
+	btVector3DoubleData			m_bvhAabbMin;
+	btVector3DoubleData			m_bvhAabbMax;
+	btVector3DoubleData			m_bvhQuantization;
+	int							m_curNodeIndex;
+	int							m_useQuantization;
+	int							m_numContiguousLeafNodes;
+	int							m_numQuantizedContiguousNodes;
+	btOptimizedBvhNodeDoubleData	*m_contiguousNodesPtr;
+	btQuantizedBvhNodeData			*m_quantizedContiguousNodesPtr;
+
+	int							m_traversalMode;
+	int							m_numSubtreeHeaders;
+	btBvhSubtreeInfoData		*m_subTreeInfoPtr;
+};
+
+
+SIMD_FORCE_INLINE	int	btQuantizedBvh::calculateSerializeBufferSizeNew() const
+{
+	return sizeof(btQuantizedBvhData);
+}
+
+
+
+#endif //BT_QUANTIZED_BVH_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

@@ -0,0 +1,349 @@
+/*
+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 "LinearMath/btAabbUtil2.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;
+	m_LastHandleIndex = -1;
+	
+
+	{
+		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, const btVector3& aabbMin,const btVector3& aabbMax)
+{
+	for (int i=0; i <= m_LastHandleIndex; i++)
+	{
+		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+		if(!proxy->m_clientObject)
+		{
+			continue;
+		}
+		rayCallback.process(proxy);
+	}
+}
+
+
+void	btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
+{
+	for (int i=0; i <= m_LastHandleIndex; i++)
+	{
+		btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
+		if(!proxy->m_clientObject)
+		{
+			continue;
+		}
+		if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax))
+		{
+			callback.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)
+	{
+		int new_largest_index = -1;
+		for (i=0; i <= m_LastHandleIndex; i++)
+		{
+			btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
+			if(!proxy0->m_clientObject)
+			{
+				continue;
+			}
+			new_largest_index = i;
+			for (j=i+1; j <= m_LastHandleIndex; j++)
+			{
+				btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
+				btAssert(proxy0 != proxy1);
+				if(!proxy1->m_clientObject)
+				{
+					continue;
+				}
+
+				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);
+						}
+					}
+				}
+			}
+		}
+
+		m_LastHandleIndex = new_largest_index;
+
+		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);
+}
+
+void	btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
+{
+	//not yet
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h

@@ -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.
+*/
+
+#ifndef BT_SIMPLE_BROADPHASE_H
+#define BT_SIMPLE_BROADPHASE_H
+
+
+#include "btOverlappingPairCache.h"
+
+
+struct btSimpleBroadphaseProxy : public btBroadphaseProxy
+{
+	int			m_nextFree;
+	
+//	int			m_handleId;
+
+	
+	btSimpleBroadphaseProxy() {};
+
+	btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& 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
+	int		m_LastHandleIndex;							
+	
+	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++;
+		if(freeHandle > m_LastHandleIndex)
+		{
+			m_LastHandleIndex = freeHandle;
+		}
+		return freeHandle;
+	}
+
+	void freeHandle(btSimpleBroadphaseProxy* proxy)
+	{
+		int handle = int(proxy-m_pHandles);
+		btAssert(handle >= 0 && handle < m_maxHandles);
+		if(handle == m_LastHandleIndex)
+		{
+			m_LastHandleIndex--;
+		}
+		proxy->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		proxy->m_clientObject = 0;
+
+		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;
+	}
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(btDispatcher* dispatcher);
+
+
+	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, const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0));
+	virtual void	aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
+		
+	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(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
+		aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
+	}
+
+	virtual void	printStats()
+	{
+//		printf("btSimpleBroadphase.h\n");
+//		printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
+	}
+};
+
+
+
+#endif //BT_SIMPLE_BROADPHASE_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CMakeLists.txt

@@ -0,0 +1,288 @@
+INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src  )
+
+SET(BulletCollision_SRCS
+	BroadphaseCollision/btAxisSweep3.cpp
+	BroadphaseCollision/btBroadphaseProxy.cpp
+	BroadphaseCollision/btCollisionAlgorithm.cpp
+	BroadphaseCollision/btDbvt.cpp
+	BroadphaseCollision/btDbvtBroadphase.cpp
+	BroadphaseCollision/btDispatcher.cpp
+	BroadphaseCollision/btMultiSapBroadphase.cpp
+	BroadphaseCollision/btOverlappingPairCache.cpp
+	BroadphaseCollision/btQuantizedBvh.cpp
+	BroadphaseCollision/btSimpleBroadphase.cpp
+	CollisionDispatch/btActivatingCollisionAlgorithm.cpp
+	CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+	CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp
+	CollisionDispatch/btBoxBoxDetector.cpp
+	CollisionDispatch/btCollisionDispatcher.cpp
+	CollisionDispatch/btCollisionObject.cpp
+	CollisionDispatch/btCollisionWorld.cpp
+	CollisionDispatch/btCollisionWorldImporter.cpp
+	CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+	CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+	CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+	CollisionDispatch/btConvexConvexAlgorithm.cpp
+	CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+	CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
+	CollisionDispatch/btDefaultCollisionConfiguration.cpp
+	CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+	CollisionDispatch/btGhostObject.cpp
+	CollisionDispatch/btHashedSimplePairCache.cpp
+	CollisionDispatch/btInternalEdgeUtility.cpp
+	CollisionDispatch/btInternalEdgeUtility.h
+	CollisionDispatch/btManifoldResult.cpp
+	CollisionDispatch/btSimulationIslandManager.cpp
+	CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+	CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+	CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+	CollisionDispatch/btUnionFind.cpp
+	CollisionDispatch/SphereTriangleDetector.cpp
+	CollisionShapes/btBoxShape.cpp
+	CollisionShapes/btBox2dShape.cpp
+	CollisionShapes/btBvhTriangleMeshShape.cpp
+	CollisionShapes/btCapsuleShape.cpp
+	CollisionShapes/btCollisionShape.cpp
+	CollisionShapes/btCompoundShape.cpp
+	CollisionShapes/btConcaveShape.cpp
+	CollisionShapes/btConeShape.cpp
+	CollisionShapes/btConvexHullShape.cpp
+	CollisionShapes/btConvexInternalShape.cpp
+	CollisionShapes/btConvexPointCloudShape.cpp
+	CollisionShapes/btConvexPolyhedron.cpp
+	CollisionShapes/btConvexShape.cpp
+	CollisionShapes/btConvex2dShape.cpp
+	CollisionShapes/btConvexTriangleMeshShape.cpp
+	CollisionShapes/btCylinderShape.cpp
+	CollisionShapes/btEmptyShape.cpp
+	CollisionShapes/btHeightfieldTerrainShape.cpp
+	CollisionShapes/btMinkowskiSumShape.cpp
+	CollisionShapes/btMultimaterialTriangleMeshShape.cpp
+	CollisionShapes/btMultiSphereShape.cpp
+	CollisionShapes/btOptimizedBvh.cpp
+	CollisionShapes/btPolyhedralConvexShape.cpp
+	CollisionShapes/btScaledBvhTriangleMeshShape.cpp
+	CollisionShapes/btShapeHull.cpp
+	CollisionShapes/btSphereShape.cpp
+	CollisionShapes/btStaticPlaneShape.cpp
+	CollisionShapes/btStridingMeshInterface.cpp
+	CollisionShapes/btTetrahedronShape.cpp
+	CollisionShapes/btTriangleBuffer.cpp
+	CollisionShapes/btTriangleCallback.cpp
+	CollisionShapes/btTriangleIndexVertexArray.cpp
+	CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
+	CollisionShapes/btTriangleMesh.cpp
+	CollisionShapes/btTriangleMeshShape.cpp
+	CollisionShapes/btUniformScalingShape.cpp
+	Gimpact/btContactProcessing.cpp
+	Gimpact/btGenericPoolAllocator.cpp
+	Gimpact/btGImpactBvh.cpp
+	Gimpact/btGImpactCollisionAlgorithm.cpp
+	Gimpact/btGImpactQuantizedBvh.cpp
+	Gimpact/btGImpactShape.cpp
+	Gimpact/btTriangleShapeEx.cpp
+	Gimpact/gim_box_set.cpp
+	Gimpact/gim_contact.cpp
+	Gimpact/gim_memory.cpp
+	Gimpact/gim_tri_collision.cpp
+	NarrowPhaseCollision/btContinuousConvexCollision.cpp
+	NarrowPhaseCollision/btConvexCast.cpp
+	NarrowPhaseCollision/btGjkConvexCast.cpp
+	NarrowPhaseCollision/btGjkEpa2.cpp
+	NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+	NarrowPhaseCollision/btGjkPairDetector.cpp
+	NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+	NarrowPhaseCollision/btPersistentManifold.cpp
+	NarrowPhaseCollision/btRaycastCallback.cpp
+	NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+	NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+	NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+)
+
+SET(Root_HDRS
+	../btBulletCollisionCommon.h
+)
+SET(BroadphaseCollision_HDRS
+	BroadphaseCollision/btAxisSweep3.h
+	BroadphaseCollision/btBroadphaseInterface.h
+	BroadphaseCollision/btBroadphaseProxy.h
+	BroadphaseCollision/btCollisionAlgorithm.h
+	BroadphaseCollision/btDbvt.h
+	BroadphaseCollision/btDbvtBroadphase.h
+	BroadphaseCollision/btDispatcher.h
+	BroadphaseCollision/btMultiSapBroadphase.h
+	BroadphaseCollision/btOverlappingPairCache.h
+	BroadphaseCollision/btOverlappingPairCallback.h
+	BroadphaseCollision/btQuantizedBvh.h
+	BroadphaseCollision/btSimpleBroadphase.h
+)
+SET(CollisionDispatch_HDRS
+	CollisionDispatch/btActivatingCollisionAlgorithm.h
+	CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+	CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h
+	CollisionDispatch/btBoxBoxDetector.h
+	CollisionDispatch/btCollisionConfiguration.h
+	CollisionDispatch/btCollisionCreateFunc.h
+	CollisionDispatch/btCollisionDispatcher.h
+	CollisionDispatch/btCollisionObject.h
+	CollisionDispatch/btCollisionObjectWrapper.h
+	CollisionDispatch/btCollisionWorld.h
+	CollisionDispatch/btCollisionWorldImporter.h
+	CollisionDispatch/btCompoundCollisionAlgorithm.h
+	CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
+	CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+	CollisionDispatch/btConvexConvexAlgorithm.h
+	CollisionDispatch/btConvex2dConvex2dAlgorithm.h
+	CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
+	CollisionDispatch/btDefaultCollisionConfiguration.h
+	CollisionDispatch/btEmptyCollisionAlgorithm.h
+	CollisionDispatch/btGhostObject.h
+	CollisionDispatch/btHashedSimplePairCache.h
+	CollisionDispatch/btManifoldResult.h
+	CollisionDispatch/btSimulationIslandManager.h
+	CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+	CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+	CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
+	CollisionDispatch/btUnionFind.h
+	CollisionDispatch/SphereTriangleDetector.h
+)
+SET(CollisionShapes_HDRS
+	CollisionShapes/btBoxShape.h
+	CollisionShapes/btBox2dShape.h
+	CollisionShapes/btBvhTriangleMeshShape.h
+	CollisionShapes/btCapsuleShape.h
+	CollisionShapes/btCollisionMargin.h
+	CollisionShapes/btCollisionShape.h
+	CollisionShapes/btCompoundShape.h
+	CollisionShapes/btConcaveShape.h
+	CollisionShapes/btConeShape.h
+	CollisionShapes/btConvexHullShape.h
+	CollisionShapes/btConvexInternalShape.h
+	CollisionShapes/btConvexPointCloudShape.h
+	CollisionShapes/btConvexPolyhedron.h
+	CollisionShapes/btConvexShape.h
+	CollisionShapes/btConvex2dShape.h
+	CollisionShapes/btConvexTriangleMeshShape.h
+	CollisionShapes/btCylinderShape.h
+	CollisionShapes/btEmptyShape.h
+	CollisionShapes/btHeightfieldTerrainShape.h
+	CollisionShapes/btMaterial.h
+	CollisionShapes/btMinkowskiSumShape.h
+	CollisionShapes/btMultimaterialTriangleMeshShape.h
+	CollisionShapes/btMultiSphereShape.h
+	CollisionShapes/btOptimizedBvh.h
+	CollisionShapes/btPolyhedralConvexShape.h
+	CollisionShapes/btScaledBvhTriangleMeshShape.h
+	CollisionShapes/btShapeHull.h
+	CollisionShapes/btSphereShape.h
+	CollisionShapes/btStaticPlaneShape.h
+	CollisionShapes/btStridingMeshInterface.h
+	CollisionShapes/btTetrahedronShape.h
+	CollisionShapes/btTriangleBuffer.h
+	CollisionShapes/btTriangleCallback.h
+	CollisionShapes/btTriangleIndexVertexArray.h
+	CollisionShapes/btTriangleIndexVertexMaterialArray.h
+	CollisionShapes/btTriangleInfoMap.h
+	CollisionShapes/btTriangleMesh.h
+	CollisionShapes/btTriangleMeshShape.h
+	CollisionShapes/btTriangleShape.h
+	CollisionShapes/btUniformScalingShape.h
+)
+SET(Gimpact_HDRS
+	Gimpact/btBoxCollision.h
+	Gimpact/btClipPolygon.h
+	Gimpact/btContactProcessing.h
+	Gimpact/btGenericPoolAllocator.h
+	Gimpact/btGeometryOperations.h
+	Gimpact/btGImpactBvh.h
+	Gimpact/btGImpactCollisionAlgorithm.h
+	Gimpact/btGImpactMassUtil.h
+	Gimpact/btGImpactQuantizedBvh.h
+	Gimpact/btGImpactShape.h
+	Gimpact/btQuantization.h
+	Gimpact/btTriangleShapeEx.h
+	Gimpact/gim_array.h
+	Gimpact/gim_basic_geometry_operations.h
+	Gimpact/gim_bitset.h
+	Gimpact/gim_box_collision.h
+	Gimpact/gim_box_set.h
+	Gimpact/gim_clip_polygon.h
+	Gimpact/gim_contact.h
+	Gimpact/gim_geom_types.h
+	Gimpact/gim_geometry.h
+	Gimpact/gim_hash_table.h
+	Gimpact/gim_linear_math.h
+	Gimpact/gim_math.h
+	Gimpact/gim_memory.h
+	Gimpact/gim_radixsort.h
+	Gimpact/gim_tri_collision.h
+)
+SET(NarrowPhaseCollision_HDRS
+	NarrowPhaseCollision/btContinuousConvexCollision.h
+	NarrowPhaseCollision/btConvexCast.h
+	NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
+	NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
+	NarrowPhaseCollision/btGjkConvexCast.h
+	NarrowPhaseCollision/btGjkEpa2.h
+	NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
+	NarrowPhaseCollision/btGjkPairDetector.h
+	NarrowPhaseCollision/btManifoldPoint.h
+	NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
+	NarrowPhaseCollision/btPersistentManifold.h
+	NarrowPhaseCollision/btPointCollector.h
+	NarrowPhaseCollision/btRaycastCallback.h
+	NarrowPhaseCollision/btSimplexSolverInterface.h
+	NarrowPhaseCollision/btSubSimplexConvexCast.h
+	NarrowPhaseCollision/btVoronoiSimplexSolver.h
+	NarrowPhaseCollision/btPolyhedralContactClipping.h
+)
+
+SET(BulletCollision_HDRS
+	${Root_HDRS}
+	${BroadphaseCollision_HDRS}
+	${CollisionDispatch_HDRS}
+	${CollisionShapes_HDRS}
+	${Gimpact_HDRS}
+	${NarrowPhaseCollision_HDRS}
+)
+
+
+ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS} ${BulletCollision_HDRS})
+SET_TARGET_PROPERTIES(BulletCollision PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(BulletCollision PROPERTIES SOVERSION ${BULLET_VERSION})
+IF (BUILD_SHARED_LIBS)
+  TARGET_LINK_LIBRARIES(BulletCollision LinearMath)
+ENDIF (BUILD_SHARED_LIBS)
+
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#INSTALL of other files requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletCollision DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletCollision RUNTIME DESTINATION bin
+								LIBRARY DESTINATION lib${LIB_SUFFIX}
+								ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+				INSTALL(FILES ../btBulletCollisionCommon.h
+DESTINATION ${INCLUDE_INSTALL_DIR}/BulletCollision)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(BulletCollision PROPERTIES FRAMEWORK true)
+
+			SET_TARGET_PROPERTIES(BulletCollision PROPERTIES PUBLIC_HEADER "${Root_HDRS}")
+			# Have to list out sub-directories manually:
+			SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision)
+			SET_PROPERTY(SOURCE ${CollisionDispatch_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionDispatch)
+			SET_PROPERTY(SOURCE ${CollisionShapes_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionShapes)
+			SET_PROPERTY(SOURCE ${Gimpact_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Gimpact)
+			SET_PROPERTY(SOURCE ${NarrowPhaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/NarrowPhaseCollision)
+
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -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 "LinearMath/btScalar.h"
+#include "SphereTriangleDetector.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
+:m_sphere(sphere),
+m_triangle(triangle),
+m_contactBreakingThreshold(contactBreakingThreshold)
+{
+
+}
+
+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(BT_LARGE_FLOAT);
+	//move sphere into triangle space
+	btTransform	sphereInTr = transformB.inverseTimes(transformA);
+
+	if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
+	{
+		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);
+		}
+	}
+
+}
+
+
+
+// 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);
+}
+
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
+{
+
+	const btVector3* vertices = &m_triangle->getVertexPtr(0);
+	
+	btScalar radius = m_sphere->getRadius();
+	btScalar radiusWithThreshold = radius + contactBreakingThreshold;
+
+	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+	normal.normalize();
+	btVector3 p1ToCentre = sphereCenter - vertices[0];
+	btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+	if (distanceFromPlane < btScalar(0.))
+	{
+		//triangle facing the other way
+		distanceFromPlane *= btScalar(-1.);
+		normal *= btScalar(-1.);
+	}
+
+	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+	
+	// Check for contact / intersection
+	bool hasContact = false;
+	btVector3 contactPoint;
+	if (isInsideContactPlane) {
+		if (facecontains(sphereCenter,vertices,normal)) {
+			// Inside the contact wedge - touches a point on the shell plane
+			hasContact = true;
+			contactPoint = sphereCenter - normal*distanceFromPlane;
+		} else {
+			// Could be inside one of the contact capsules
+			btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+			btVector3 nearestOnEdge;
+			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+				
+				btVector3 pa;
+				btVector3 pb;
+				
+				m_triangle->getEdge(i,pa,pb);
+
+				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
+				if (distanceSqr < contactCapsuleRadiusSqr) {
+					// Yep, we're inside a capsule
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+				}
+				
+			}
+		}
+	}
+
+	if (hasContact) {
+		btVector3 contactToCentre = sphereCenter - contactPoint;
+		btScalar distanceSqr = contactToCentre.length2();
+
+		if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
+		{
+			if (distanceSqr>SIMD_EPSILON)
+			{
+				btScalar distance = btSqrt(distanceSqr);
+				resultNormal = contactToCentre;
+				resultNormal.normalize();
+				point = contactPoint;
+				depth = -(radius-distance);
+			} else
+			{
+				resultNormal = normal;
+				point = contactPoint;
+				depth = -radius;
+			}
+			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;
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

@@ -0,0 +1,51 @@
+/*
+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_SPHERE_TRIANGLE_DETECTOR_H
+#define BT_SPHERE_TRIANGLE_DETECTOR_H
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.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, btScalar contactBreakingThreshold);
+
+	virtual ~SphereTriangleDetector() {};
+
+	bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar	contactBreakingThreshold);
+
+private:
+
+	
+	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;
+	btScalar	m_contactBreakingThreshold;
+	
+};
+#endif //BT_SPHERE_TRIANGLE_DETECTOR_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp

@@ -0,0 +1,47 @@
+/*
+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 "btActivatingCollisionAlgorithm.h"
+#include "btCollisionDispatcher.h"
+#include "btCollisionObject.h"
+
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
+:btCollisionAlgorithm(ci)
+//,
+//m_colObj0(0),
+//m_colObj1(0)
+{
+}
+btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* )
+:btCollisionAlgorithm(ci)
+//,
+//m_colObj0(0),
+//m_colObj1(0)
+{
+//	if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
+//	{
+//		m_colObj0 = colObj0;
+//		m_colObj1 = colObj1;
+//		
+//		m_colObj0->activate();
+//		m_colObj1->activate();
+//	}
+}
+
+btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
+{
+//		m_colObj0->activate();
+//		m_colObj1->activate();
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h

@@ -0,0 +1,36 @@
+/*
+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.
+*/
+
+#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
+#define __BT_ACTIVATING_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
+class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
+{
+//	btCollisionObject* m_colObj0;
+//	btCollisionObject* m_colObj1;
+
+public:
+
+	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
+
+	btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+	virtual ~btActivatingCollisionAlgorithm();
+
+};
+#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp

@@ -0,0 +1,421 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.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.
+*/
+
+///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library.
+///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross
+
+#include "btBox2dBox2dCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
+#include "BulletCollision/CollisionShapes/btBox2dShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+#define USE_PERSISTENT_CONTACTS 1
+
+btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
+: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
+{
+	
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+	
+}
+
+
+void b2CollidePolygons(btManifoldResult* manifold,  const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
+
+//#include <stdio.h>
+void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	
+	const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
+	const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
+
+	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
+{
+	//not yet
+	return 1.f;
+}
+
+
+struct ClipVertex
+{
+	btVector3 v;
+	int id;
+	//b2ContactID id;
+	//b2ContactID id;
+};
+
+#define b2Dot(a,b) (a).dot(b)
+#define b2Mul(a,b) (a)*(b)
+#define b2MulT(a,b) (a).transpose()*(b)
+#define b2Cross(a,b) (a).cross(b)
+#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
+
+int b2_maxManifoldPoints =2;
+
+static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
+					  const btVector3& normal, btScalar offset)
+{
+	// Start with no output points
+	int numOut = 0;
+
+	// Calculate the distance of end points to the line
+	btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
+	btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+	// If the points are behind the plane
+	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+	// If the points are on different sides of the plane
+	if (distance0 * distance1 < 0.0f)
+	{
+		// Find intersection point of edge and plane
+		btScalar interp = distance0 / (distance0 - distance1);
+		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+		if (distance0 > 0.0f)
+		{
+			vOut[numOut].id = vIn[0].id;
+		}
+		else
+		{
+			vOut[numOut].id = vIn[1].id;
+		}
+		++numOut;
+	}
+
+	return numOut;
+}
+
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+							  const btBox2dShape* poly2, const btTransform& xf2)
+{
+	const btVector3* vertices1 = poly1->getVertices();
+	const btVector3* normals1 = poly1->getNormals();
+
+	int count2 = poly2->getVertexCount();
+	const btVector3* vertices2 = poly2->getVertices();
+
+	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
+
+	// Convert normal from poly1's frame into poly2's frame.
+	btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
+	btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
+
+	// Find support vertex on poly2 for -normal.
+	int index = 0;
+	btScalar minDot = BT_LARGE_FLOAT;
+
+    if( count2 > 0 )
+        index = (int) normal1.minDot( vertices2, count2, minDot);
+
+	btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
+	btVector3 v2 = b2Mul(xf2, vertices2[index]);
+	btScalar separation = b2Dot(v2 - v1, normal1World);
+	return separation;
+}
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static btScalar FindMaxSeparation(int* edgeIndex,
+								 const btBox2dShape* poly1, const btTransform& xf1,
+								 const btBox2dShape* poly2, const btTransform& xf2)
+{
+	int count1 = poly1->getVertexCount();
+	const btVector3* normals1 = poly1->getNormals();
+
+	// Vector pointing from the centroid of poly1 to the centroid of poly2.
+	btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
+	btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
+
+	// Find edge normal on poly1 that has the largest projection onto d.
+	int edge = 0;
+    btScalar maxDot;
+    if( count1 > 0 )
+        edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
+
+	// Get the separation for the edge normal.
+	btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+	if (s > 0.0f)
+	{
+		return s;
+	}
+
+	// Check the separation for the previous edge normal.
+	int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+	btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+	if (sPrev > 0.0f)
+	{
+		return sPrev;
+	}
+
+	// Check the separation for the next edge normal.
+	int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+	btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+	if (sNext > 0.0f)
+	{
+		return sNext;
+	}
+
+	// Find the best edge and the search direction.
+	int bestEdge;
+	btScalar bestSeparation;
+	int increment;
+	if (sPrev > s && sPrev > sNext)
+	{
+		increment = -1;
+		bestEdge = prevEdge;
+		bestSeparation = sPrev;
+	}
+	else if (sNext > s)
+	{
+		increment = 1;
+		bestEdge = nextEdge;
+		bestSeparation = sNext;
+	}
+	else
+	{
+		*edgeIndex = edge;
+		return s;
+	}
+
+	// Perform a local search for the best edge normal.
+	for ( ; ; )
+	{
+		if (increment == -1)
+			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+		else
+			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+		s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+		if (s > 0.0f)
+		{
+			return s;
+		}
+
+		if (s > bestSeparation)
+		{
+			bestEdge = edge;
+			bestSeparation = s;
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	*edgeIndex = bestEdge;
+	return bestSeparation;
+}
+
+static void FindIncidentEdge(ClipVertex c[2],
+							 const btBox2dShape* poly1, const btTransform& xf1, int edge1,
+							 const btBox2dShape* poly2, const btTransform& xf2)
+{
+	const btVector3* normals1 = poly1->getNormals();
+
+	int count2 = poly2->getVertexCount();
+	const btVector3* vertices2 = poly2->getVertices();
+	const btVector3* normals2 = poly2->getNormals();
+
+	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
+
+	// Get the normal of the reference edge in poly2's frame.
+	btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
+
+	// Find the incident edge on poly2.
+	int index = 0;
+	btScalar minDot = BT_LARGE_FLOAT;
+	for (int i = 0; i < count2; ++i)
+	{
+		btScalar dot = b2Dot(normal1, normals2[i]);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	int i1 = index;
+	int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+	c[0].v = b2Mul(xf2, vertices2[i1]);
+//	c[0].id.features.referenceEdge = (unsigned char)edge1;
+//	c[0].id.features.incidentEdge = (unsigned char)i1;
+//	c[0].id.features.incidentVertex = 0;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+//	c[1].id.features.referenceEdge = (unsigned char)edge1;
+//	c[1].id.features.incidentEdge = (unsigned char)i2;
+//	c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(btManifoldResult* manifold,
+					  const btBox2dShape* polyA, const btTransform& xfA,
+					  const btBox2dShape* polyB, const btTransform& xfB)
+{
+
+	int edgeA = 0;
+	btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > 0.0f)
+		return;
+
+	int edgeB = 0;
+	btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > 0.0f)
+		return;
+
+	const btBox2dShape* poly1;	// reference poly
+	const btBox2dShape* poly2;	// incident poly
+	btTransform xf1, xf2;
+	int edge1;		// reference edge
+	unsigned char flip;
+	const btScalar k_relativeTol = 0.98f;
+	const btScalar k_absoluteTol = 0.001f;
+
+	// TODO_ERIN use "radius" of poly for absolute tolerance.
+	if (separationB > k_relativeTol * separationA + k_absoluteTol)
+	{
+		poly1 = polyB;
+		poly2 = polyA;
+		xf1 = xfB;
+		xf2 = xfA;
+		edge1 = edgeB;
+		flip = 1;
+	}
+	else
+	{
+		poly1 = polyA;
+		poly2 = polyB;
+		xf1 = xfA;
+		xf2 = xfB;
+		edge1 = edgeA;
+		flip = 0;
+	}
+
+	ClipVertex incidentEdge[2];
+	FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+	int count1 = poly1->getVertexCount();
+	const btVector3* vertices1 = poly1->getVertices();
+
+	btVector3 v11 = vertices1[edge1];
+	btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+
+	//btVector3 dv = v12 - v11;
+	btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
+	sideNormal.normalize();
+	btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
+	
+	
+	v11 = b2Mul(xf1, v11);
+	v12 = b2Mul(xf1, v12);
+
+	btScalar frontOffset = b2Dot(frontNormal, v11);
+	btScalar sideOffset1 = -b2Dot(sideNormal, v11);
+	btScalar sideOffset2 = b2Dot(sideNormal, v12);
+
+	// Clip incident edge against extruded edge1 side edges.
+	ClipVertex clipPoints1[2];
+	clipPoints1[0].v.setValue(0,0,0);
+	clipPoints1[1].v.setValue(0,0,0);
+
+	ClipVertex clipPoints2[2];
+	clipPoints2[0].v.setValue(0,0,0);
+	clipPoints2[1].v.setValue(0,0,0);
+
+
+	int np;
+
+	// Clip to box side 1
+	np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
+
+	if (np < 2)
+	{
+		return;
+	}
+
+	// Now clipPoints2 contains the clipped points.
+	btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
+
+	int pointCount = 0;
+	for (int i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+
+		if (separation <= 0.0f)
+		{
+			
+			//b2ManifoldPoint* cp = manifold->points + pointCount;
+			//btScalar separation = separation;
+			//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
+			//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+
+			manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
+
+//			cp->id = clipPoints2[i].id;
+//			cp->id.features.flip = flip;
+			++pointCount;
+		}
+	}
+
+//	manifold->pointCount = pointCount;}
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h

@@ -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 BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+#define BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+
+class btPersistentManifold;
+
+///box-box collision detection
+class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+	virtual ~btBox2dBox2dCollisionAlgorithm();
+
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
+		}
+	};
+
+};
+
+#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp

@@ -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 "btBoxBoxCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "btBoxBoxDetector.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+#define USE_PERSISTENT_CONTACTS 1
+
+btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	
+	const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape();
+	const btBoxShape* box1 = (btBoxShape*)body1Wrap->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 = BT_LARGE_FLOAT;
+	input.m_transformA = body0Wrap->getWorldTransform();
+	input.m_transformB = body1Wrap->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;
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h

@@ -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 BT_BOX_BOX__COLLISION_ALGORITHM_H
+#define BT_BOX_BOX__COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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 btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+	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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
+			void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
+			return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap);
+		}
+	};
+
+};
+
+#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

@@ -0,0 +1,718 @@
+/*
+ * 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: [email protected]   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(const btBoxShape* box1,const 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];
+	if (btFabs(a+q) > SIMD_EPSILON)
+	{
+		a = 1.f/(btScalar(3.0)*(a+q));
+	} else
+	{
+		a=BT_LARGE_FLOAT;
+	}
+    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(0.f,0.f,0.f);
+  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 > SIMD_EPSILON) return 0; \
+  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+  if (l > SIMD_EPSILON) { \
+    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); \
+    } \
+  }
+
+  btScalar fudge2 (1.0e-5f);
+
+  Q11 += fudge2;
+  Q12 += fudge2;
+  Q13 += fudge2;
+
+  Q21 += fudge2;
+  Q22 += fudge2;
+  Q23 += fudge2;
+
+  Q31 += fudge2;
+  Q32 += fudge2;
+  Q33 += fudge2;
+
+  // 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) {
+
+	  if (code<4) 
+	  {
+    // we have less contacts than we need, so we use them all
+    for (j=0; j < cnum; 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 less contacts than we need, so we use them all
+		for (j=0; j < cnum; j++) 
+		{
+			btVector3 pointInWorld;
+			for (i=0; i<3; i++) 
+				pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+				//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];
+		if (code<4) 
+	   {
+			output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+		} else
+		{
+			output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-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
+	);
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h

@@ -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: [email protected]   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 BT_BOX_BOX_DETECTOR_H
+#define BT_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
+{
+	const btBoxShape* m_box1;
+	const btBoxShape* m_box2;
+
+public:
+
+	btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2);
+
+	virtual ~btBoxBoxDetector() {};
+
+	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
+
+};
+
+#endif //BT_BOX_BOX_DETECTOR_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h

@@ -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 BT_COLLISION_CONFIGURATION
+#define BT_COLLISION_CONFIGURATION
+
+struct btCollisionAlgorithmCreateFunc;
+
+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 btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
+
+};
+
+#endif //BT_COLLISION_CONFIGURATION
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h

@@ -0,0 +1,45 @@
+/*
+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_CREATE_FUNC
+#define BT_COLLISION_CREATE_FUNC
+
+#include "LinearMath/btAlignedObjectArray.h"
+class btCollisionAlgorithm;
+class btCollisionObject;
+struct btCollisionObjectWrapper;
+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& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+	{
+		
+		(void)body0Wrap;
+		(void)body1Wrap;
+		return 0;
+	}
+};
+#endif //BT_COLLISION_CREATE_FUNC
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -0,0 +1,315 @@
+/*
+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 "LinearMath/btQuickprof.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"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+int gNumManifold = 0;
+
+#ifdef BT_DEBUG
+#include <stdio.h>
+#endif
+
+
+btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
+m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
+	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);
+			btAssert(m_doubleDispatch[i][j]);
+		}
+	}
+	
+	
+}
+
+
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+}
+
+btCollisionDispatcher::~btCollisionDispatcher()
+{
+}
+
+btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) 
+{ 
+	gNumManifold++;
+	
+	//btAssert(gNumManifold < 65535);
+	
+
+
+	//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+	
+	btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
+		btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+		: gContactBreakingThreshold ;
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
+		
+ 	void* mem = 0;
+	
+	if (m_persistentManifoldPoolAllocator->getFreeCount())
+	{
+		mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	} else
+	{
+		//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
+		if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0)
+		{
+			mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
+		} else
+		{
+			btAssert(0);
+			//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
+			return 0;
+		}
+	}
+	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
+	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(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
+{
+	
+	btCollisionAlgorithmConstructionInfo ci;
+
+	ci.m_dispatcher1 = this;
+	ci.m_manifold = sharedManifold;
+	btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
+
+	return algo;
+}
+
+
+
+
+bool	btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const 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(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btAssert(body0);
+	btAssert(body1);
+
+	bool needsCollision = true;
+
+#ifdef BT_DEBUG
+	if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
+	{
+		//broadphase filtering already deals with this
+		if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject())
+		{
+			m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
+			printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+		}
+	}
+#endif //BT_DEBUG
+
+	if ((!body0->isActive()) && (!body1->isActive()))
+		needsCollision = false;
+	else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
+		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)
+	{
+		BT_PROFILE("btCollisionDispatcher::processOverlap");
+
+		(*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))
+		{
+			btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1);
+			btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1);
+
+
+			//dispatcher will keep algorithms persistent in the collision pair
+			if (!collisionPair.m_algorithm)
+			{
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
+			}
+
+			if (collisionPair.m_algorithm)
+			{
+				btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap);
+				
+				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
+				{
+					//discrete collision detection query
+					
+					collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,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);
+	}
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -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.
+*/
+
+#ifndef BT_COLLISION__DISPATCHER_H
+#define BT_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
+{
+
+protected:
+
+	int		m_dispatcherFlags;
+
+	btAlignedObjectArray<btPersistentManifold*>	m_manifoldsPtr;
+
+	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:
+
+	enum DispatcherFlags
+	{
+		CD_STATIC_STATIC_REPORTED = 1,
+		CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2,
+		CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4
+	};
+
+	int	getDispatcherFlags() const
+	{
+		return m_dispatcherFlags;
+	}
+
+	void	setDispatcherFlags(int flags)
+	{
+		m_dispatcherFlags = flags;
+	}
+
+	///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.size()? &m_manifoldsPtr[0] : 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(const btCollisionObject* b0,const btCollisionObject* b1);
+	
+	virtual void releaseManifold(btPersistentManifold* manifold);
+
+
+	virtual void clearManifold(btPersistentManifold* manifold);
+
+	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0);
+		
+	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
+	
+	virtual bool	needsResponse(const btCollisionObject* body0,const 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;
+	}
+
+	virtual	btPoolAllocator*	getInternalManifoldPool()
+	{
+		return m_persistentManifoldPoolAllocator;
+	}
+
+	virtual	const btPoolAllocator*	getInternalManifoldPool() const
+	{
+		return m_persistentManifoldPoolAllocator;
+	}
+
+};
+
+#endif //BT_COLLISION__DISPATCHER_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -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 "btCollisionObject.h"
+#include "LinearMath/btSerializer.h"
+
+btCollisionObject::btCollisionObject()
+	:	m_anisotropicFriction(1.f,1.f,1.f),
+	m_hasAnisotropicFriction(false),
+	m_contactProcessingThreshold(BT_LARGE_FLOAT),
+		m_broadphaseHandle(0),
+		m_collisionShape(0),
+		m_extensionPointer(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_rollingFriction(0.0f),
+        m_spinningFriction(0.f),
+		m_contactDamping(.1),
+		m_contactStiffness(1e4),
+		m_internalType(CO_COLLISION_OBJECT),
+		m_userObjectPointer(0),
+		m_userIndex2(-1),
+		m_userIndex(-1),
+		m_hitFraction(btScalar(1.)),
+		m_ccdSweptSphereRadius(btScalar(0.)),
+		m_ccdMotionThreshold(btScalar(0.)),
+		m_checkCollideWith(false),
+		m_updateRevision(0)
+{
+	m_worldTransform.setIdentity();
+}
+
+btCollisionObject::~btCollisionObject()
+{
+}
+
+void btCollisionObject::setActivationState(int newState) const
+{ 
+	if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
+		m_activationState1 = newState;
+}
+
+void btCollisionObject::forceActivationState(int newState) const
+{
+	m_activationState1 = newState;
+}
+
+void btCollisionObject::activate(bool forceActivation) const
+{
+	if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
+	{
+		setActivationState(ACTIVE_TAG);
+		m_deactivationTime = btScalar(0.);
+	}
+}
+
+const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
+
+	m_worldTransform.serialize(dataOut->m_worldTransform);
+	m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform);
+	m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity);
+	m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity);
+	m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction);
+	dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction;
+	dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
+	dataOut->m_broadphaseHandle = 0;
+	dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
+	dataOut->m_rootCollisionShape = 0;//@todo
+	dataOut->m_collisionFlags = m_collisionFlags;
+	dataOut->m_islandTag1 = m_islandTag1;
+	dataOut->m_companionId = m_companionId;
+	dataOut->m_activationState1 = m_activationState1;
+	dataOut->m_deactivationTime = m_deactivationTime;
+	dataOut->m_friction = m_friction;
+	dataOut->m_rollingFriction = m_rollingFriction;
+	dataOut->m_contactDamping = m_contactDamping;
+	dataOut->m_contactStiffness = m_contactStiffness;
+	dataOut->m_restitution = m_restitution;
+	dataOut->m_internalType = m_internalType;
+	
+	char* name = (char*) serializer->findNameForPointer(this);
+	dataOut->m_name = (char*)serializer->getUniquePointer(name);
+	if (dataOut->m_name)
+	{
+		serializer->serializeName(name);
+	}
+	dataOut->m_hitFraction = m_hitFraction;
+	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
+	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+	dataOut->m_checkCollideWith = m_checkCollideWith;
+
+	return btCollisionObjectDataName;
+}
+
+
+void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
+{
+	int len = calculateSerializeBufferSize();
+	btChunk* chunk = serializer->allocate(len,1);
+	const char* structType = serialize(chunk->m_oldPtr, serializer);
+	serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -0,0 +1,643 @@
+/*
+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_OBJECT_H
+#define BT_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;
+struct btCollisionShapeData;
+#include "LinearMath/btMotionState.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#define btCollisionObjectDataName "btCollisionObjectDoubleData"
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#define btCollisionObjectDataName "btCollisionObjectFloatData"
+#endif
+
+
+/// 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;
+	
+	btVector3	m_anisotropicFriction;
+	int			m_hasAnisotropicFriction;
+	btScalar	m_contactProcessingThreshold;	
+
+	btBroadphaseProxy*		m_broadphaseHandle;
+	btCollisionShape*		m_collisionShape;
+	///m_extensionPointer is used by some internal low-level Bullet extensions.
+	void*					m_extensionPointer;
+	
+	///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;
+
+	mutable int				m_activationState1;
+	mutable btScalar			m_deactivationTime;
+
+	btScalar		m_friction;
+	btScalar		m_restitution;
+	btScalar		m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
+    btScalar        m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
+	btScalar		m_contactDamping;
+	btScalar		m_contactStiffness;
+	
+	
+
+	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
+	///do not assign your own m_internalType unless you write a new dynamics object class.
+	int				m_internalType;
+
+	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+
+	void*			m_userObjectPointer;
+
+	int				m_userIndex2;
+	
+    int	m_userIndex;
+
+	///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
+	int			m_checkCollideWith;
+
+	btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;
+
+	///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
+	int			m_updateRevision;
+
+
+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)
+		CF_CHARACTER_OBJECT = 16,
+		CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
+		CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
+		CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
+	};
+
+	enum	CollisionObjectTypes
+	{
+		CO_COLLISION_OBJECT =1,
+		CO_RIGID_BODY=2,
+		///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
+		///It is useful for collision sensors, explosion objects, character controller etc.
+		CO_GHOST_OBJECT=4,
+		CO_SOFT_BODY=8,
+		CO_HF_FLUID=16,
+		CO_USER_TYPE=32,
+		CO_FEATHERSTONE_LINK=64
+	};
+
+	enum AnisotropicFrictionFlags
+	{
+		CF_ANISOTROPIC_FRICTION_DISABLED=0,
+		CF_ANISOTROPIC_FRICTION = 1,
+		CF_ANISOTROPIC_ROLLING_FRICTION = 2
+	};
+
+	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);
+	}
+
+	const btVector3& getAnisotropicFriction() const
+	{
+		return m_anisotropicFriction;
+	}
+	void	setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
+	{
+		m_anisotropicFriction = anisotropicFriction;
+		bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+		m_hasAnisotropicFriction = isUnity?frictionMode : 0;
+	}
+	bool	hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const
+	{
+		return (m_hasAnisotropicFriction&frictionMode)!=0;
+	}
+
+	///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
+	///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
+	void	setContactProcessingThreshold( btScalar contactProcessingThreshold)
+	{
+		m_contactProcessingThreshold = contactProcessingThreshold;
+	}
+	btScalar	getContactProcessingThreshold() const
+	{
+		return m_contactProcessingThreshold;
+	}
+
+	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_updateRevision++;
+		m_collisionShape = collisionShape;
+		m_rootCollisionShape = collisionShape;
+	}
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const
+	{
+		return m_collisionShape;
+	}
+
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape()
+	{
+		return m_collisionShape;
+	}
+
+	void	setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
+	{
+		if (ignoreCollisionCheck)
+		{
+			//We don't check for duplicates. Is it ok to leave that up to the user of this API?
+			//int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+			//if (index == m_objectsWithoutCollisionCheck.size())
+			//{
+			m_objectsWithoutCollisionCheck.push_back(co);
+			//}
+		}
+		else
+		{
+			m_objectsWithoutCollisionCheck.remove(co);
+		}
+		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
+	}
+
+	virtual bool	checkCollideWithOverride(const btCollisionObject*  co) const
+	{
+		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+		if (index < m_objectsWithoutCollisionCheck.size())
+		{
+			return false;
+		}
+		return true;
+	}
+
+
+	
+
+	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. 
+	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+	void*		internalGetExtensionPointer() const
+	{
+		return m_extensionPointer;
+	}
+	///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
+	///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+	void	internalSetExtensionPointer(void* pointer)
+	{
+		m_extensionPointer = pointer;
+	}
+
+	SIMD_FORCE_INLINE	int	getActivationState() const { return m_activationState1;}
+	
+	void setActivationState(int newState) const;
+
+	void	setDeactivationTime(btScalar time)
+	{
+		m_deactivationTime = time;
+	}
+	btScalar	getDeactivationTime() const
+	{
+		return m_deactivationTime;
+	}
+
+	void forceActivationState(int newState) const;
+
+	void	activate(bool forceActivation = false) const;
+
+	SIMD_FORCE_INLINE bool isActive() const
+	{
+		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+	}
+
+	void	setRestitution(btScalar rest)
+	{
+		m_updateRevision++;
+		m_restitution = rest;
+	}
+	btScalar	getRestitution() const
+	{
+		return m_restitution;
+	}
+	void	setFriction(btScalar frict)
+	{
+		m_updateRevision++;
+		m_friction = frict;
+	}
+	btScalar	getFriction() const
+	{
+		return m_friction;
+	}
+
+	void	setRollingFriction(btScalar frict)
+	{
+		m_updateRevision++;
+		m_rollingFriction = frict;
+	}
+	btScalar	getRollingFriction() const
+	{
+		return m_rollingFriction;
+	}
+    void	setSpinningFriction(btScalar frict)
+    {
+        m_updateRevision++;
+        m_spinningFriction = frict;
+    }
+    btScalar	getSpinningFriction() const
+    {
+        return m_spinningFriction;
+    }
+    void	setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
+	{
+		m_updateRevision++;
+		m_contactStiffness = stiffness;
+		m_contactDamping = damping;
+		
+		m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
+		
+        //avoid divisions by zero...
+		if (m_contactStiffness< SIMD_EPSILON)
+        {
+            m_contactStiffness = SIMD_EPSILON;
+        }
+	}
+	
+	btScalar	getContactStiffness() const
+	{
+		return m_contactStiffness;
+	}
+	
+	btScalar	getContactDamping() const
+	{
+		return m_contactDamping;
+	}
+    
+	///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_updateRevision++;
+		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_updateRevision++;
+		m_interpolationWorldTransform = trans;
+	}
+
+	void	setInterpolationLinearVelocity(const btVector3& linvel)
+	{
+		m_updateRevision++;
+		m_interpolationLinearVelocity = linvel;
+	}
+
+	void	setInterpolationAngularVelocity(const btVector3& angvel)
+	{
+		m_updateRevision++;
+		m_interpolationAngularVelocity = angvel;
+	}
+
+	const btVector3&	getInterpolationLinearVelocity() const
+	{
+		return m_interpolationLinearVelocity;
+	}
+
+	const btVector3&	getInterpolationAngularVelocity() const
+	{
+		return m_interpolationAngularVelocity;
+	}
+
+	SIMD_FORCE_INLINE int getIslandTag() const
+	{
+		return	m_islandTag1;
+	}
+
+	void	setIslandTag(int tag)
+	{
+		m_islandTag1 = tag;
+	}
+
+	SIMD_FORCE_INLINE int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
+	SIMD_FORCE_INLINE btScalar			getHitFraction() const
+	{
+		return m_hitFraction; 
+	}
+
+	void	setHitFraction(btScalar hitFraction)
+	{
+		m_hitFraction = hitFraction;
+	}
+
+	
+	SIMD_FORCE_INLINE 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;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+
+	int	getUserIndex() const
+	{
+		return m_userIndex;
+	}
+	
+	int	getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+	
+	void	setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
+	int	getUpdateRevisionInternal() const
+	{
+		return m_updateRevision;
+	}
+
+
+	inline bool checkCollideWith(const btCollisionObject* co) const
+	{
+		if (m_checkCollideWith)
+			return checkCollideWithOverride(co);
+
+		return true;
+	}
+
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, class btSerializer* serializer) const;
+
+	virtual void serializeSingleObject(class btSerializer* serializer) const;
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCollisionObjectDoubleData
+{
+	void					*m_broadphaseHandle;
+	void					*m_collisionShape;
+	btCollisionShapeData	*m_rootCollisionShape;
+	char					*m_name;
+
+	btTransformDoubleData	m_worldTransform;
+	btTransformDoubleData	m_interpolationWorldTransform;
+	btVector3DoubleData		m_interpolationLinearVelocity;
+	btVector3DoubleData		m_interpolationAngularVelocity;
+	btVector3DoubleData		m_anisotropicFriction;
+	double					m_contactProcessingThreshold;	
+	double					m_deactivationTime;
+	double					m_friction;
+	double					m_rollingFriction;
+	double                  m_contactDamping;
+	double                  m_contactStiffness;
+	double					m_restitution;
+	double					m_hitFraction; 
+	double					m_ccdSweptSphereRadius;
+	double					m_ccdMotionThreshold;
+
+	int						m_hasAnisotropicFriction;
+	int						m_collisionFlags;
+	int						m_islandTag1;
+	int						m_companionId;
+	int						m_activationState1;
+	int						m_internalType;
+	int						m_checkCollideWith;
+
+	char	m_padding[4];
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCollisionObjectFloatData
+{
+	void					*m_broadphaseHandle;
+	void					*m_collisionShape;
+	btCollisionShapeData	*m_rootCollisionShape;
+	char					*m_name;
+
+	btTransformFloatData	m_worldTransform;
+	btTransformFloatData	m_interpolationWorldTransform;
+	btVector3FloatData		m_interpolationLinearVelocity;
+	btVector3FloatData		m_interpolationAngularVelocity;
+	btVector3FloatData		m_anisotropicFriction;
+	float					m_contactProcessingThreshold;	
+	float					m_deactivationTime;
+	float					m_friction;
+	float					m_rollingFriction;
+    float                   m_contactDamping;
+    float                   m_contactStiffness;
+	float					m_restitution;
+	float					m_hitFraction; 
+	float					m_ccdSweptSphereRadius;
+	float					m_ccdMotionThreshold;
+
+	int						m_hasAnisotropicFriction;
+	int						m_collisionFlags;
+	int						m_islandTag1;
+	int						m_companionId;
+	int						m_activationState1;
+	int						m_internalType;
+	int						m_checkCollideWith;
+	char					m_padding[4];
+};
+
+
+
+SIMD_FORCE_INLINE	int	btCollisionObject::calculateSerializeBufferSize() const
+{
+	return sizeof(btCollisionObjectData);
+}
+
+
+
+#endif //BT_COLLISION_OBJECT_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h

@@ -0,0 +1,43 @@
+#ifndef BT_COLLISION_OBJECT_WRAPPER_H
+#define BT_COLLISION_OBJECT_WRAPPER_H
+
+///btCollisionObjectWrapperis an internal data structure. 
+///Most users can ignore this and use btCollisionObject and btCollisionShape instead
+class btCollisionShape;
+class btCollisionObject;
+class btTransform;
+#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition
+
+#define BT_DECLARE_STACK_ONLY_OBJECT \
+	private: \
+		void* operator new(size_t size); \
+		void operator delete(void*);
+
+struct btCollisionObjectWrapper;
+struct btCollisionObjectWrapper
+{
+BT_DECLARE_STACK_ONLY_OBJECT
+
+private:
+	btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed.
+	btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&);
+
+public:
+	const btCollisionObjectWrapper* m_parent;
+	const btCollisionShape* m_shape;
+	const btCollisionObject* m_collisionObject;
+	const btTransform& m_worldTransform;
+	int		m_partId;
+	int		m_index;
+
+	btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
+	: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform),
+	m_partId(partId), m_index(index)
+	{}
+
+	SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
+	SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }
+	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; }
+};
+
+#endif //BT_COLLISION_OBJECT_WRAPPER_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -0,0 +1,1626 @@
+/*
+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/btCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btSerializer.h"
+#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+
+
+//#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"
+
+
+///for debug drawing
+
+//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"
+
+
+
+btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
+:m_dispatcher1(dispatcher),
+m_broadphasePairCache(pairCache),
+m_debugDrawer(0),
+m_forceUpdateAllAabbs(true)
+{
+}
+
+
+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);
+			collisionObject->setBroadphaseHandle(0);
+		}
+	}
+
+
+}
+
+
+
+
+
+
+
+
+
+
+void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+
+	btAssert(collisionObject);
+
+	//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::updateSingleAabb(btCollisionObject* colObj)
+{
+	btVector3 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;
+
+	if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
+	{
+		btVector3 minAabb2,maxAabb2;
+		colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
+		minAabb2 -= contactThreshold;
+		maxAabb2 += contactThreshold;
+		minAabb.setMin(minAabb2);
+		maxAabb.setMax(maxAabb2);
+	}
+
+	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 [email protected]\n");
+			m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+			m_debugDrawer->reportErrorWarning("Thanks.\n");
+		}
+	}
+}
+
+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 (m_forceUpdateAllAabbs || colObj->isActive())
+		{
+			updateSingleAabb(colObj);
+		}
+	}
+}
+
+
+void	btCollisionWorld::computeOverlappingPairs()
+{
+	BT_PROFILE("calculateOverlappingPairs");
+	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+}
+
+void	btCollisionWorld::performDiscreteCollisionDetection()
+{
+	BT_PROFILE("performDiscreteCollisionDetection");
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	updateAabbs();
+
+	computeOverlappingPairs();
+
+	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)
+{
+	btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
+	btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback);
+}
+
+void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+										const btCollisionObjectWrapper* collisionObjectWrap,
+										RayResultCallback& resultCallback)
+{
+	btSphereShape pointShape(btScalar(0.0));
+	pointShape.setMargin(0.f);
+	const btConvexShape* castShape = &pointShape;
+	const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape();
+	const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform();
+
+	if (collisionShape->isConvex())
+	{
+		//		BT_PROFILE("rayTestConvex");
+		btConvexCast::CastResult castResult;
+		castResult.m_fraction = resultCallback.m_closestHitFraction;
+
+		btConvexShape* convexShape = (btConvexShape*) collisionShape;
+		btVoronoiSimplexSolver	simplexSolver;
+		btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver);
+		
+		btGjkConvexCast	gjkConvexCaster(castShape,convexShape,&simplexSolver);
+		
+		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+
+		btConvexCast* convexCasterPtr = 0;
+		//use kF_UseSubSimplexConvexCastRaytest by default
+		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
+			convexCasterPtr = &gjkConvexCaster;
+		else
+			convexCasterPtr = &subSimplexConvexCaster;
+		
+		btConvexCast& convexCaster = *convexCasterPtr;
+
+		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)
+				{
+					//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
+#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
+						(
+						collisionObjectWrap->getCollisionObject(),
+						0,
+						castResult.m_normal,
+						castResult.m_fraction
+						);
+
+					bool normalInWorldSpace = true;
+					resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
+
+				}
+			}
+		}
+	} else {
+		if (collisionShape->isConcave())
+		{
+
+			//ConvexCast::CastResult
+				struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
+				{
+					btCollisionWorld::RayResultCallback* m_resultCallback;
+					const btCollisionObject*	m_collisionObject;
+					const btConcaveShape*	m_triangleMesh;
+
+					btTransform m_colObjWorldTransform;
+
+					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+					btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape*	triangleMesh,const btTransform& colObjWorldTransform):
+						//@BP Mod
+						btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+							m_resultCallback(resultCallback),
+							m_collisionObject(collisionObject),
+							m_triangleMesh(triangleMesh),
+							m_colObjWorldTransform(colObjWorldTransform)
+						{
+						}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+
+						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
+						btCollisionWorld::LocalRayResult rayResult
+							(m_collisionObject,
+							&shapeInfo,
+							hitNormalWorld,
+							hitFraction);
+
+						bool	normalInWorldSpace = true;
+						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+					}
+
+				};
+
+			btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+			btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+			btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+			//			BT_PROFILE("rayTestConcave");
+			if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				///optimized version for btBvhTriangleMeshShape
+				btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+				
+				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
+			}
+			else
+			{
+				//generic (slower) case
+				btConcaveShape* concaveShape = (btConcaveShape*)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;
+					const btCollisionObject*	m_collisionObject;
+					btConcaveShape*	m_triangleMesh;
+
+					btTransform m_colObjWorldTransform;
+
+					BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
+						btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*	triangleMesh, const btTransform& colObjWorldTransform):
+					//@BP Mod
+					btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+						m_resultCallback(resultCallback),
+						m_collisionObject(collisionObject),
+						m_triangleMesh(triangleMesh),
+						m_colObjWorldTransform(colObjWorldTransform)
+					{
+					}
+
+
+					virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
+					{
+						btCollisionWorld::LocalShapeInfo	shapeInfo;
+						shapeInfo.m_shapePart = partId;
+						shapeInfo.m_triangleIndex = triangleIndex;
+
+						btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
+						btCollisionWorld::LocalRayResult rayResult
+							(m_collisionObject,
+							&shapeInfo,
+							hitNormalWorld,
+							hitFraction);
+
+						bool	normalInWorldSpace = true;
+						return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
+					}
+
+				};
+
+
+				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+
+				btVector3 rayAabbMinLocal = rayFromLocal;
+				rayAabbMinLocal.setMin(rayToLocal);
+				btVector3 rayAabbMaxLocal = rayFromLocal;
+				rayAabbMaxLocal.setMax(rayToLocal);
+
+				concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
+			}
+		} else {
+			//			BT_PROFILE("rayTestCompound");
+			if (collisionShape->isCompound())
+			{
+				struct LocalInfoAdder2 : public RayResultCallback
+				{
+					RayResultCallback* m_userCallback;
+					int m_i;
+					
+					LocalInfoAdder2 (int i, RayResultCallback *user)
+						: m_userCallback(user), m_i(i)
+					{ 
+						m_closestHitFraction = m_userCallback->m_closestHitFraction;
+						m_flags = m_userCallback->m_flags;
+					}
+					virtual bool needsCollision(btBroadphaseProxy* p) const
+					{
+						return m_userCallback->needsCollision(p);
+					}
+
+					virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
+					{
+						btCollisionWorld::LocalShapeInfo shapeInfo;
+						shapeInfo.m_shapePart = -1;
+						shapeInfo.m_triangleIndex = m_i;
+						if (r.m_localShapeInfo == NULL)
+							r.m_localShapeInfo = &shapeInfo;
+
+						const btScalar result = m_userCallback->addSingleResult(r, b);
+						m_closestHitFraction = m_userCallback->m_closestHitFraction;
+						return result;
+					}
+				};
+				
+				struct RayTester : btDbvt::ICollide
+				{
+					const btCollisionObject* m_collisionObject;
+					const btCompoundShape* m_compoundShape;
+					const btTransform& m_colObjWorldTransform;
+					const btTransform& m_rayFromTrans;
+					const btTransform& m_rayToTrans;
+					RayResultCallback& m_resultCallback;
+					
+					RayTester(const btCollisionObject* collisionObject,
+							const btCompoundShape* compoundShape,
+							const btTransform& colObjWorldTransform,
+							const btTransform& rayFromTrans,
+							const btTransform& rayToTrans,
+							RayResultCallback& resultCallback):
+						m_collisionObject(collisionObject),
+						m_compoundShape(compoundShape),
+						m_colObjWorldTransform(colObjWorldTransform),
+						m_rayFromTrans(rayFromTrans),
+						m_rayToTrans(rayToTrans),
+						m_resultCallback(resultCallback)
+					{
+						
+					}
+					
+					void ProcessLeaf(int i)
+					{
+						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
+						const btTransform& childTrans = m_compoundShape->getChildTransform(i);
+						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+						
+						btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i);
+						// replace collision shape so that callback can determine the triangle
+
+						
+
+						LocalInfoAdder2 my_cb(i, &m_resultCallback);
+
+						rayTestSingleInternal(
+							m_rayFromTrans,
+							m_rayToTrans,
+							&tmpOb,
+							my_cb);
+						
+					}
+				
+					void Process(const btDbvtNode* leaf)
+					{
+						ProcessLeaf(leaf->dataAsInt);
+					}
+				};
+				
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+				const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
+
+
+				RayTester rayCB(
+					collisionObjectWrap->getCollisionObject(),
+					compoundShape,
+					colObjWorldTransform,
+					rayFromTrans,
+					rayToTrans,
+					resultCallback);
+#ifndef	DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+				if (dbvt)
+				{
+					btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
+					btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
+					btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
+				}
+				else
+#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+				{
+					for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
+					{
+						rayCB.ProcessLeaf(i);
+					}	
+				}
+			}
+		}
+	}
+}
+
+void	btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
+											btCollisionObject* collisionObject,
+											const btCollisionShape* collisionShape,
+											const btTransform& colObjWorldTransform,
+											ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+{
+	btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1);
+	btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration);
+}
+
+void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
+											const btCollisionObjectWrapper* colObjWrap,
+											ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+{
+	const btCollisionShape* collisionShape = colObjWrap->getCollisionShape();
+	const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform();
+
+	if (collisionShape->isConvex())
+	{
+		//BT_PROFILE("convexSweepConvex");
+		btConvexCast::CastResult castResult;
+		castResult.m_allowedPenetration = allowedPenetration;
+		castResult.m_fraction = resultCallback.m_closestHitFraction;//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
+						(
+						colObjWrap->getCollisionObject(),
+						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)
+			{
+				//BT_PROFILE("convexSweepbtBvhTriangleMesh");
+				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;
+					const btCollisionObject*	m_collisionObject;
+					btTriangleMeshShape*	m_triangleMesh;
+
+					BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+						btCollisionWorld::ConvexResultCallback* resultCallback, const 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,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform);
+				tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+				tccb.m_allowedPenetration = allowedPenetration;
+				btVector3 boxMinLocal, boxMaxLocal;
+				castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+				triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
+			} else
+			{
+				if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
+				{
+					btConvexCast::CastResult castResult;
+					castResult.m_allowedPenetration = allowedPenetration;
+					castResult.m_fraction = resultCallback.m_closestHitFraction;
+					btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape;
+					btContinuousConvexCollision convexCaster1(castShape,planeShape);
+					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
+									(
+									colObjWrap->getCollisionObject(),
+									0,
+									castResult.m_normal,
+									castResult.m_hitPoint,
+									castResult.m_fraction
+									);
+
+								bool normalInWorldSpace = true;
+								resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
+							}
+						}
+					}
+
+				} else
+				{
+					//BT_PROFILE("convexSweepConcave");
+					btConcaveShape* concaveShape = (btConcaveShape*)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;
+						const btCollisionObject*	m_collisionObject;
+						btConcaveShape*	m_triangleMesh;
+
+						BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
+							btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape*	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,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+					tccb.m_hitFraction = resultCallback.m_closestHitFraction;
+					tccb.m_allowedPenetration = allowedPenetration;
+					btVector3 boxMinLocal, boxMaxLocal;
+					castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
+
+					btVector3 rayAabbMinLocal = convexFromLocal;
+					rayAabbMinLocal.setMin(convexToLocal);
+					btVector3 rayAabbMaxLocal = convexFromLocal;
+					rayAabbMaxLocal.setMax(convexToLocal);
+					rayAabbMinLocal += boxMinLocal;
+					rayAabbMaxLocal += boxMaxLocal;
+					concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
+				}
+			}
+		} else {
+			if (collisionShape->isCompound())
+			{
+				struct	btCompoundLeafCallback : btDbvt::ICollide
+				{
+					btCompoundLeafCallback(
+										   const btCollisionObjectWrapper* colObjWrap,
+										   const btConvexShape* castShape,
+										   const btTransform& convexFromTrans,
+										   const btTransform& convexToTrans,
+										   btScalar allowedPenetration,
+										   const btCompoundShape* compoundShape,
+										   const btTransform& colObjWorldTransform,
+										   ConvexResultCallback& resultCallback)
+					: 
+					  m_colObjWrap(colObjWrap),
+						m_castShape(castShape),
+						m_convexFromTrans(convexFromTrans),
+						m_convexToTrans(convexToTrans),
+						m_allowedPenetration(allowedPenetration),
+						m_compoundShape(compoundShape),
+						m_colObjWorldTransform(colObjWorldTransform),
+						m_resultCallback(resultCallback) {
+					}
+
+				  const btCollisionObjectWrapper* m_colObjWrap;
+					const btConvexShape* m_castShape;
+					const btTransform& m_convexFromTrans;
+					const btTransform& m_convexToTrans;
+					btScalar m_allowedPenetration;
+					const btCompoundShape* m_compoundShape;
+					const btTransform& m_colObjWorldTransform;
+					ConvexResultCallback& m_resultCallback;
+
+				public:
+
+					void		ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape)
+					{
+						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+
+						struct	LocalInfoAdder : public ConvexResultCallback {
+							ConvexResultCallback* m_userCallback;
+							int m_i;
+
+							LocalInfoAdder(int i, ConvexResultCallback *user)
+								: m_userCallback(user), m_i(i)
+							{
+								m_closestHitFraction = m_userCallback->m_closestHitFraction;
+							}
+							virtual bool needsCollision(btBroadphaseProxy* p) const
+							{
+								return m_userCallback->needsCollision(p);
+							}
+							virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult&	r, bool b)
+							{
+								btCollisionWorld::LocalShapeInfo	shapeInfo;
+								shapeInfo.m_shapePart = -1;
+								shapeInfo.m_triangleIndex = m_i;
+								if (r.m_localShapeInfo == NULL)
+									r.m_localShapeInfo = &shapeInfo;
+								const btScalar result = m_userCallback->addSingleResult(r, b);
+								m_closestHitFraction = m_userCallback->m_closestHitFraction;
+								return result;
+
+							}
+						};
+
+						LocalInfoAdder my_cb(index, &m_resultCallback);
+
+						btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index);
+
+						objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration);
+					}
+
+					void		Process(const btDbvtNode* leaf)
+					{
+						// Processing leaf node
+						int index = leaf->dataAsInt;
+
+						btTransform childTrans = m_compoundShape->getChildTransform(index);
+						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index);
+
+						ProcessChild(index, childTrans, childCollisionShape);
+					}
+				};
+
+				BT_PROFILE("convexSweepCompound");
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+
+				btVector3 fromLocalAabbMin, fromLocalAabbMax;
+				btVector3 toLocalAabbMin, toLocalAabbMax;
+
+				castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax);
+				castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax);
+
+				fromLocalAabbMin.setMin(toLocalAabbMin);
+				fromLocalAabbMax.setMax(toLocalAabbMax);
+
+				btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans,
+					  allowedPenetration, compoundShape, colObjWorldTransform, resultCallback);
+
+				const btDbvt* tree = compoundShape->getDynamicAabbTree();
+				if (tree) {
+					const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax);
+					tree->collideTV(tree->m_root, bounds, callback);
+				} else {
+					int i;
+					for (i=0;i<compoundShape->getNumChildShapes();i++)
+					{
+						const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+						btTransform childTrans = compoundShape->getChildTransform(i);
+						callback.ProcessChild(i, childTrans, childCollisionShape);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+struct btSingleRayCallback : public btBroadphaseRayCallback
+{
+
+	btVector3	m_rayFromWorld;
+	btVector3	m_rayToWorld;
+	btTransform	m_rayFromTrans;
+	btTransform	m_rayToTrans;
+	btVector3	m_hitNormal;
+
+	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)
+	{
+		m_rayFromTrans.setIdentity();
+		m_rayFromTrans.setOrigin(m_rayFromWorld);
+		m_rayToTrans.setIdentity();
+		m_rayToTrans.setOrigin(m_rayToWorld);
+
+		btVector3 rayDir = (rayToWorld-rayFromWorld);
+
+		rayDir.normalize ();
+		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
+		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
+
+	}
+
+
+
+	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;
+#if 0
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+			//culling already done by broadphase
+			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+			{
+				m_world->rayTestSingle(m_rayFromTrans,m_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");
+	/// use the broadphase to accelerate the search for objects, based on their aabb
+	/// and for each object with ray-aabb overlap, perform an exact ray test
+	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
+
+}
+
+
+struct btSingleSweepCallback : public btBroadphaseRayCallback
+{
+
+	btTransform	m_convexFromTrans;
+	btTransform	m_convexToTrans;
+	btVector3	m_hitNormal;
+	const btCollisionWorld*	m_world;
+	btCollisionWorld::ConvexResultCallback&	m_resultCallback;
+	btScalar	m_allowedCcdPenetration;
+	const btConvexShape* m_castShape;
+
+
+	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
+		:m_convexFromTrans(convexFromTrans),
+		m_convexToTrans(convexToTrans),
+		m_world(world),
+		m_resultCallback(resultCallback),
+		m_allowedCcdPenetration(allowedPenetration),
+		m_castShape(castShape)
+	{
+		btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
+		btVector3 rayDir = unnormalizedRayDir.normalized();
+		///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
+		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+		m_lambda_max = rayDir.dot(unnormalizedRayDir);
+
+	}
+
+	virtual bool	process(const btBroadphaseProxy* proxy)
+	{
+		///terminate further convex sweep 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();
+			m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
+				collisionObject,
+				collisionObject->getCollisionShape(),
+				collisionObject->getWorldTransform(),
+				m_resultCallback,
+				m_allowedCcdPenetration);
+		}
+
+		return true;
+	}
+};
+
+
+
+void	btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
+{
+
+	BT_PROFILE("convexSweepTest");
+	/// use the broadphase to accelerate the search for objects, based on their aabb
+	/// and for each object with ray-aabb overlap, perform an exact ray test
+	/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
+
+
+
+	btTransform	convexFromTrans,convexToTrans;
+	convexFromTrans = convexFromWorld;
+	convexToTrans = convexToWorld;
+	btVector3 castShapeAabbMin, castShapeAabbMax;
+	/* Compute AABB that encompasses angular movement */
+	{
+		btVector3 linVel, angVel;
+		btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel);
+		btVector3 zeroLinVel;
+		zeroLinVel.setValue(0,0,0);
+		btTransform R;
+		R.setIdentity ();
+		R.setRotation (convexFromTrans.getRotation());
+		castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax);
+	}
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+
+	btSingleSweepCallback	convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
+
+	m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
+
+#else
+	/// 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,
+					allowedCcdPenetration);
+			}
+		}
+	}
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+}
+
+
+
+struct btBridgedManifoldResult : public btManifoldResult
+{
+
+	btCollisionWorld::ContactResultCallback&	m_resultCallback;
+
+	btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback )
+		:btManifoldResult(obj0Wrap,obj1Wrap),
+		m_resultCallback(resultCallback)
+	{
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	{
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+		btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+		btVector3 localA;
+		btVector3 localB;
+		if (isSwapped)
+		{
+			localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+			localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
+		} else
+		{
+			localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+			localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
+		}
+		
+		btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+		newPt.m_positionWorldOnA = pointA;
+		newPt.m_positionWorldOnB = pointInWorld;
+		
+	   //BP mod, store contact triangles.
+		if (isSwapped)
+		{
+			newPt.m_partId0 = m_partId1;
+			newPt.m_partId1 = m_partId0;
+			newPt.m_index0  = m_index1;
+			newPt.m_index1  = m_index0;
+		} else
+		{
+			newPt.m_partId0 = m_partId0;
+			newPt.m_partId1 = m_partId1;
+			newPt.m_index0  = m_index0;
+			newPt.m_index1  = m_index1;
+		}
+
+		//experimental feature info, for per-triangle material etc.
+		const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
+		const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
+		m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
+
+	}
+	
+};
+
+
+
+struct btSingleContactCallback : public btBroadphaseAabbCallback
+{
+
+	btCollisionObject* m_collisionObject;
+	btCollisionWorld*	m_world;
+	btCollisionWorld::ContactResultCallback&	m_resultCallback;
+	
+	
+	btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
+		:m_collisionObject(collisionObject),
+		m_world(world),
+		m_resultCallback(resultCallback)
+	{
+	}
+
+	virtual bool	process(const btBroadphaseProxy* proxy)
+	{
+		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+		if (collisionObject == m_collisionObject)
+			return true;
+
+		//only perform raycast if filterMask matches
+		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
+			btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
+
+			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
+			if (algorithm)
+			{
+				btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
+				//discrete collision detection query
+				
+				algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult);
+
+				algorithm->~btCollisionAlgorithm();
+				m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
+			}
+		}
+		return true;
+	}
+};
+
+
+///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
+///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+void	btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
+{
+	btVector3 aabbMin,aabbMax;
+	colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
+	btSingleContactCallback	contactCB(colObj,this,resultCallback);
+	
+	m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
+}
+
+
+///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+///it reports one or more contact points (including the one with deepest penetration)
+void	btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
+{
+	btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
+	btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
+
+	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
+	if (algorithm)
+	{
+		btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
+		//discrete collision detection query
+		algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
+
+		algorithm->~btCollisionAlgorithm();
+		getDispatcher()->freeCollisionAlgorithm(algorithm);
+	}
+
+}
+
+
+
+
+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];
+		  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
+          
+          if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals )
+          {
+		    btVector3 normal = (wv1-wv0).cross(wv2-wv0);
+		    normal.normalize();
+		    btVector3 normalColor(1,1,0);
+		    m_debugDrawer->drawLine(center,center+normal,normalColor);
+          }
+		  m_debugDrawer->drawLine(wv0,wv1,m_color);
+		  m_debugDrawer->drawLine(wv1,wv2,m_color);
+		  m_debugDrawer->drawLine(wv2,wv0,m_color);
+	  }
+};
+
+
+void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+	// Draw a small simplex at the center of the object
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
+	{
+		getDebugDrawer()->drawTransform(worldTransform,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 BOX_SHAPE_PROXYTYPE:
+            {
+                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
+                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
+                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
+                break;
+            }
+
+        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
+
+                getDebugDrawer()->drawSphere(radius, worldTransform, color);
+                break;
+            }
+        case MULTI_SPHERE_SHAPE_PROXYTYPE:
+            {
+                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+                btTransform childTransform;
+                childTransform.setIdentity();
+
+                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+                {
+                    childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+                    getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*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();
+                getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, 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();
+
+                int upAxis= coneShape->getConeUpIndex();
+                getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, 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];
+                getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
+                break;
+            }
+
+        case STATIC_PLANE_PROXYTYPE:
+            {
+                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+                btScalar planeConst = staticPlaneShape->getPlaneConstant();
+                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+                getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
+                break;
+
+            }
+        default:
+            {
+
+                /// for polyhedral shapes
+                if (shape->isPolyhedral())
+                {
+                    btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+                    
+                    int i;
+                    if (polyshape->getConvexPolyhedron())
+                    {
+                        const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron();
+                        for (i=0;i<poly->m_faces.size();i++)
+                        {
+                            btVector3 centroid(0,0,0);
+                            int numVerts = poly->m_faces[i].m_indices.size();
+                            if (numVerts)
+                            {
+                                int lastV = poly->m_faces[i].m_indices[numVerts-1];
+                                for (int v=0;v<poly->m_faces[i].m_indices.size();v++)
+                                {
+                                    int curVert = poly->m_faces[i].m_indices[v];
+                                    centroid+=poly->m_vertices[curVert];
+                                    getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color);
+                                    lastV = curVert;
+                                }
+                            }
+                            centroid*= btScalar(1.f)/btScalar(numVerts);
+                            if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals)
+                            {
+                                btVector3 normalColor(1,1,0);
+                                btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
+                                getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
+                            }
+                            
+                        }
+                        
+                        
+                    } else
+                    {
+                        for (i=0;i<polyshape->getNumEdges();i++)
+                        {
+                            btVector3 a,b;
+                            polyshape->getEdge(i,a,b);
+                            btVector3 wa = worldTransform * a;
+                            btVector3 wb = worldTransform * b;
+                            getDebugDrawer()->drawLine(wa,wb,color);
+                        }
+                    }
+                    
+                    
+                }
+                    
+                if (shape->isConcave())
+                {
+                    btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+
+                    ///@todo pass camera, for some culling? no -> we are not a graphics lib
+                    btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                    btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+                    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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                    btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                    //DebugDrawcallback drawCallback;
+                    DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+                    convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+                }
+
+
+                
+            }
+       
+		}
+	}
+}
+
+
+void	btCollisionWorld::debugDrawWorld()
+{
+	if (getDebugDrawer())
+	{
+		btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
+
+		if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+		{
+		
+
+			if (getDispatcher())
+			{
+				int numManifolds = getDispatcher()->getNumManifolds();
+			
+				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(),defaultColors.m_contactPoint);
+					}
+				}
+			}
+		}
+
+		if ((getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
+		{
+			int i;
+
+			for (  i=0;i<m_collisionObjects.size();i++)
+			{
+				btCollisionObject* colObj = m_collisionObjects[i];
+				if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
+				{
+					if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
+					{
+						btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4));
+
+						switch(colObj->getActivationState())
+						{
+						case  ACTIVE_TAG:
+							color = defaultColors.m_activeObject; break;
+						case ISLAND_SLEEPING:
+							color =  defaultColors.m_deactivatedObject;break;
+						case WANTS_DEACTIVATION:
+							color = defaultColors.m_wantsDeactivationObject;break;
+						case DISABLE_DEACTIVATION:
+							color = defaultColors.m_disabledDeactivationObject;break;
+						case DISABLE_SIMULATION:
+							color = defaultColors.m_disabledSimulationObject;break;
+						default:
+							{
+								color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3));
+							}
+						};
+
+						debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+					}
+					if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+					{
+						btVector3 minAabb,maxAabb;
+						btVector3 colorvec = defaultColors.m_aabb;
+						colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+						btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+						minAabb -= contactThreshold;
+						maxAabb += contactThreshold;
+
+						btVector3 minAabb2,maxAabb2;
+
+						if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
+						{
+							colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
+							minAabb2 -= contactThreshold;
+							maxAabb2 += contactThreshold;
+							minAabb.setMin(minAabb2);
+							maxAabb.setMax(maxAabb2);
+						}
+
+						m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+					}
+				}
+			}
+		}
+	}
+}
+
+
+void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
+{
+	int i;
+
+	///keep track of shapes already serialized
+	btHashMap<btHashPtr,btCollisionShape*>	serializedShapes;
+
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		btCollisionShape* shape = colObj->getCollisionShape();
+
+		if (!serializedShapes.find(shape))
+		{
+			serializedShapes.insert(shape,shape);
+			shape->serializeSingleShape(serializer);
+		}
+	}
+
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
+		{
+			colObj->serializeSingleObject(serializer);
+		}
+	}
+}
+
+
+void	btCollisionWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+	
+	serializeCollisionObjects(serializer);
+	
+	serializer->finishSerialization();
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -0,0 +1,526 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 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 ).
+ *
+ * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
+ * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
+ * Please visit http://www.bulletphysics.org
+ *
+ * @section install_sec Installation
+ *
+ * @subsection step1 Step 1: Download
+ * You can download the Bullet Physics Library from the github repository: https://github.com/bulletphysics/bullet3/releases 
+ *
+ * @subsection step2 Step 2: Building
+ * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.
+ * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. 
+ * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. 
+ * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects:
+ * cd Bullet/build
+ * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4
+ * cd Bullet/build/gmake
+ * make
+ * 
+ * An alternative to premake is cmake. You can download cmake from http://www.cmake.org
+ * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
+ * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
+ * You can also use cmake in the command-line. Here are some examples for various platforms:
+ * cmake . -G "Visual Studio 9 2008"
+ * cmake . -G Xcode
+ * cmake . -G "Unix Makefiles"
+ * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make.
+ * 
+ * @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
+ * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
+ * 
+ */
+ 
+ 
+
+#ifndef BT_COLLISION_WORLD_H
+#define BT_COLLISION_WORLD_H
+
+class btCollisionShape;
+class btConvexShape;
+class btBroadphaseInterface;
+class btSerializer;
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btCollisionObject.h"
+#include "btCollisionDispatcher.h"
+#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;
+
+	btBroadphaseInterface*	m_broadphasePairCache;
+
+	btIDebugDraw*	m_debugDrawer;
+
+	///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
+	///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
+	bool m_forceUpdateAllAabbs;
+
+	void	serializeCollisionObjects(btSerializer* serializer);
+
+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;
+	}
+
+	void	updateSingleAabb(btCollisionObject* colObj);
+
+	virtual void	updateAabbs();
+
+	///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation)
+	///it can be useful to use if you perform ray tests without collision detection/simulation
+	virtual void	computeOverlappingPairs();
+
+	
+	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
+	{
+			m_debugDrawer = debugDrawer;
+	}
+
+	virtual btIDebugDraw*	getDebugDrawer()
+	{
+		return m_debugDrawer;
+	}
+
+	virtual void	debugDrawWorld();
+
+	virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+
+
+	///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(const btCollisionObject*	collisionObject, 
+			LocalShapeInfo*	localShapeInfo,
+			const btVector3&		hitNormalLocal,
+			btScalar hitFraction)
+		:m_collisionObject(collisionObject),
+		m_localShapeInfo(localShapeInfo),
+		m_hitNormalLocal(hitNormalLocal),
+		m_hitFraction(hitFraction)
+		{
+		}
+
+		const 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;
+		const btCollisionObject*		m_collisionObject;
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+		//@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke.
+		unsigned int m_flags;
+
+		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),
+			//@BP Mod
+			m_flags(0)
+		{
+		}
+
+		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	AllHitsRayResultCallback : public RayResultCallback
+	{
+		AllHitsRayResultCallback(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
+		:m_rayFromWorld(rayFromWorld),
+		m_rayToWorld(rayToWorld)
+		{
+		}
+
+		btAlignedObjectArray<const btCollisionObject*>		m_collisionObjects;
+
+		btVector3	m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+		btVector3	m_rayToWorld;
+
+		btAlignedObjectArray<btVector3>	m_hitNormalWorld;
+		btAlignedObjectArray<btVector3>	m_hitPointWorld;
+		btAlignedObjectArray<btScalar> m_hitFractions;
+			
+		virtual	btScalar	addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+		{
+			m_collisionObject = rayResult.m_collisionObject;
+			m_collisionObjects.push_back(rayResult.m_collisionObject);
+			btVector3 hitNormalWorld;
+			if (normalInWorldSpace)
+			{
+				hitNormalWorld = rayResult.m_hitNormalLocal;
+			} else
+			{
+				///need to transform normal into worldspace
+				hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+			}
+			m_hitNormalWorld.push_back(hitNormalWorld);
+			btVector3 hitPointWorld;
+			hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+			m_hitPointWorld.push_back(hitPointWorld);
+			m_hitFractions.push_back(rayResult.m_hitFraction);
+			return m_closestHitFraction;
+		}
+	};
+
+
+	struct LocalConvexResult
+	{
+		LocalConvexResult(const 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)
+		{
+		}
+
+		const 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;
+		const 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;
+		}
+	};
+
+	///ContactResultCallback is used to report contact points
+	struct	ContactResultCallback
+	{
+		short int	m_collisionFilterGroup;
+		short int	m_collisionFilterMask;
+		
+		ContactResultCallback()
+			:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+			m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+		{
+		}
+
+		virtual ~ContactResultCallback()
+		{
+		}
+		
+		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(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0;
+	};
+
+
+
+	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.
+	virtual 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,  btScalar allowedCcdPenetration = btScalar(0.)) const;
+
+	///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
+	///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+	void	contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
+
+	///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+	///it reports one or more contact points (including the one with deepest penetration)
+	void	contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
+
+
+	/// 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);
+
+	static void	rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  const btCollisionObjectWrapper* collisionObjectWrap,
+					  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);
+
+	static void	objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
+											const btCollisionObjectWrapper* colObjWrap,
+											ConvexResultCallback& resultCallback, btScalar allowedPenetration);
+
+	virtual 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;
+	}
+
+
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void	performDiscreteCollisionDetection();
+
+	btDispatcherInfo& getDispatchInfo()
+	{
+		return m_dispatchInfo;
+	}
+
+	const btDispatcherInfo& getDispatchInfo() const
+	{
+		return m_dispatchInfo;
+	}
+	
+	bool	getForceUpdateAllAabbs() const
+	{
+		return m_forceUpdateAllAabbs;
+	}
+	void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+	{
+		m_forceUpdateAllAabbs = forceUpdateAllAabbs;
+	}
+
+	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
+	virtual	void	serialize(btSerializer* serializer);
+
+};
+
+
+#endif //BT_COLLISION_WORLD_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp

@@ -0,0 +1,1147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 "btCollisionWorldImporter.h"
+#include "btBulletCollisionCommon.h"
+#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition
+
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+#include "BulletCollision/Gimpact/btGImpactShape.h"
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+
+btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world)
+:m_collisionWorld(world),
+m_verboseMode(0)
+{
+
+}
+
+btCollisionWorldImporter::~btCollisionWorldImporter()
+{
+}
+
+
+
+
+
+bool	btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays)
+{
+
+	m_shapeMap.clear();
+	m_bodyMap.clear();
+
+	int i;
+
+	for (i=0;i<arrays->m_bvhsDouble.size();i++)
+	{
+		btOptimizedBvh* bvh = createOptimizedBvh();
+		btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i];
+		bvh->deSerializeDouble(*bvhData);
+		m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh);
+	}
+	for (i=0;i<arrays->m_bvhsFloat.size();i++)
+    {
+        btOptimizedBvh* bvh = createOptimizedBvh();
+   		btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i];
+		bvh->deSerializeFloat(*bvhData);
+		m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh);
+	}
+
+
+
+
+
+	for (i=0;i<arrays->m_colShapeData.size();i++)
+	{
+		btCollisionShapeData* shapeData = arrays->m_colShapeData[i];
+		btCollisionShape* shape = convertCollisionShape(shapeData);
+		if (shape)
+		{
+	//		printf("shapeMap.insert(%x,%x)\n",shapeData,shape);
+			m_shapeMap.insert(shapeData,shape);
+		}
+
+		if (shape&& shapeData->m_name)
+		{
+			char* newname = duplicateName(shapeData->m_name);
+			m_objectNameMap.insert(shape,newname);
+			m_nameShapeMap.insert(newname,shape);
+		}
+	}
+
+
+	for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++)
+	{
+        btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i];
+        btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape);
+        if (shapePtr && *shapePtr)
+        {
+            btTransform startTransform;
+            colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
+            startTransform.deSerializeDouble(colObjData->m_worldTransform);
+
+            btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+            btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
+            body->setFriction(btScalar(colObjData->m_friction));
+            body->setRestitution(btScalar(colObjData->m_restitution));
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+            if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+            {
+                btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
+                if (trimesh->getTriangleInfoMap())
+                {
+                    body->setCollisionFlags(body->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+                }
+            }
+#endif //USE_INTERNAL_EDGE_UTILITY
+            m_bodyMap.insert(colObjData,body);
+        } else
+        {
+            printf("error: no shape found\n");
+        }
+	}
+	for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++)
+	{
+        btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i];
+        btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape);
+        if (shapePtr && *shapePtr)
+        {
+            btTransform startTransform;
+            colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f;
+            startTransform.deSerializeFloat(colObjData->m_worldTransform);
+
+            btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+            btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name);
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+            if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+            {
+                btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
+                if (trimesh->getTriangleInfoMap())
+                {
+                    body->setCollisionFlags(body->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+                }
+            }
+#endif //USE_INTERNAL_EDGE_UTILITY
+            m_bodyMap.insert(colObjData,body);
+        } else
+        {
+            printf("error: no shape found\n");
+        }
+    }
+
+	return true;
+}
+
+
+
+void btCollisionWorldImporter::deleteAllData()
+{
+	int i;
+
+	for (i=0;i<m_allocatedCollisionObjects.size();i++)
+	{
+		if(m_collisionWorld)
+			m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]);
+		delete m_allocatedCollisionObjects[i];
+	}
+
+	m_allocatedCollisionObjects.clear();
+
+
+	for (i=0;i<m_allocatedCollisionShapes.size();i++)
+	{
+		delete m_allocatedCollisionShapes[i];
+	}
+	m_allocatedCollisionShapes.clear();
+
+
+	for (i=0;i<m_allocatedBvhs.size();i++)
+	{
+		delete m_allocatedBvhs[i];
+	}
+	m_allocatedBvhs.clear();
+
+	for (i=0;i<m_allocatedTriangleInfoMaps.size();i++)
+	{
+		delete m_allocatedTriangleInfoMaps[i];
+	}
+	m_allocatedTriangleInfoMaps.clear();
+	for (i=0;i<m_allocatedTriangleIndexArrays.size();i++)
+	{
+		delete m_allocatedTriangleIndexArrays[i];
+	}
+	m_allocatedTriangleIndexArrays.clear();
+	for (i=0;i<m_allocatedNames.size();i++)
+	{
+		delete[] m_allocatedNames[i];
+	}
+	m_allocatedNames.clear();
+
+	for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++)
+	{
+		btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i];
+
+		for(int a = 0;a < curData->m_numMeshParts;a++)
+		{
+			btMeshPartData* curPart = &curData->m_meshPartsPtr[a];
+			if(curPart->m_vertices3f)
+				delete [] curPart->m_vertices3f;
+
+			if(curPart->m_vertices3d)
+				delete [] curPart->m_vertices3d;
+
+			if(curPart->m_indices32)
+				delete [] curPart->m_indices32;
+
+			if(curPart->m_3indices16)
+				delete [] curPart->m_3indices16;
+
+			if(curPart->m_indices16)
+				delete [] curPart->m_indices16;
+
+			if (curPart->m_3indices8)
+				delete [] curPart->m_3indices8;
+
+		}
+		delete [] curData->m_meshPartsPtr;
+		delete curData;
+	}
+	m_allocatedbtStridingMeshInterfaceDatas.clear();
+
+	for (i=0;i<m_indexArrays.size();i++)
+	{
+		btAlignedFree(m_indexArrays[i]);
+	}
+  m_indexArrays.clear();
+
+	for (i=0;i<m_shortIndexArrays.size();i++)
+	{
+		btAlignedFree(m_shortIndexArrays[i]);
+	}
+  m_shortIndexArrays.clear();
+
+	for (i=0;i<m_charIndexArrays.size();i++)
+	{
+		btAlignedFree(m_charIndexArrays[i]);
+	}
+  m_charIndexArrays.clear();
+
+	for (i=0;i<m_floatVertexArrays.size();i++)
+	{
+		btAlignedFree(m_floatVertexArrays[i]);
+	}
+  m_floatVertexArrays.clear();
+
+	for (i=0;i<m_doubleVertexArrays.size();i++)
+	{
+		btAlignedFree(m_doubleVertexArrays[i]);
+	}
+   m_doubleVertexArrays.clear();
+
+
+}
+
+
+
+btCollisionShape* btCollisionWorldImporter::convertCollisionShape(  btCollisionShapeData* shapeData  )
+{
+	btCollisionShape* shape = 0;
+
+	switch (shapeData->m_shapeType)
+		{
+	case STATIC_PLANE_PROXYTYPE:
+		{
+			btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData;
+			btVector3 planeNormal,localScaling;
+			planeNormal.deSerializeFloat(planeData->m_planeNormal);
+			localScaling.deSerializeFloat(planeData->m_localScaling);
+			shape = createPlaneShape(planeNormal,planeData->m_planeConstant);
+			shape->setLocalScaling(localScaling);
+
+			break;
+		}
+	case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData;
+			btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData;
+			colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+			btCollisionShape* childShape = convertCollisionShape(colShapeData);
+			btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape;
+			btVector3 localScaling;
+			localScaling.deSerializeFloat(scaledMesh->m_localScaling);
+
+			shape = createScaledTrangleMeshShape(meshShape, localScaling);
+			break;
+		}
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+	case GIMPACT_SHAPE_PROXYTYPE:
+		{
+			btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
+			if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE)
+			{
+				btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface);
+				btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData);
+
+
+				btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface);
+				btVector3 localScaling;
+				localScaling.deSerializeFloat(gimpactData->m_localScaling);
+				gimpactShape->setLocalScaling(localScaling);
+				gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin));
+				gimpactShape->updateBound();
+				shape = gimpactShape;
+			} else
+			{
+				printf("unsupported gimpact sub type\n");
+			}
+			break;
+		}
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+	//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API
+	//so deal with this
+		case CAPSULE_SHAPE_PROXYTYPE:
+		{
+			btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData;
+
+
+			switch (capData->m_upAxis)
+			{
+			case 0:
+				{
+					shape = createCapsuleShapeX(1,1);
+					break;
+				}
+			case 1:
+				{
+					shape = createCapsuleShapeY(1,1);
+					break;
+				}
+			case 2:
+				{
+					shape = createCapsuleShapeZ(1,1);
+					break;
+				}
+			default:
+				{
+					printf("error: wrong up axis for btCapsuleShape\n");
+				}
+
+
+			};
+			if (shape)
+			{
+				btCapsuleShape* cap = (btCapsuleShape*) shape;
+				cap->deSerializeFloat(capData);
+			}
+			break;
+		}
+		case CYLINDER_SHAPE_PROXYTYPE:
+		case CONE_SHAPE_PROXYTYPE:
+		case BOX_SHAPE_PROXYTYPE:
+		case SPHERE_SHAPE_PROXYTYPE:
+		case MULTI_SPHERE_SHAPE_PROXYTYPE:
+		case CONVEX_HULL_SHAPE_PROXYTYPE:
+			{
+				btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData;
+				btVector3 implicitShapeDimensions;
+				implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions);
+				btVector3 localScaling;
+				localScaling.deSerializeFloat(bsd->m_localScaling);
+				btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin);
+				switch (shapeData->m_shapeType)
+				{
+					case BOX_SHAPE_PROXYTYPE:
+						{
+							btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin);
+							//box->initializePolyhedralFeatures();
+							shape = box;
+
+							break;
+						}
+					case SPHERE_SHAPE_PROXYTYPE:
+						{
+							shape = createSphereShape(implicitShapeDimensions.getX());
+							break;
+						}
+
+					case CYLINDER_SHAPE_PROXYTYPE:
+						{
+							btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData;
+							btVector3 halfExtents = implicitShapeDimensions+margin;
+							switch (cylData->m_upAxis)
+							{
+							case 0:
+								{
+									shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX());
+									break;
+								}
+							case 1:
+								{
+									shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY());
+									break;
+								}
+							case 2:
+								{
+									shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ());
+									break;
+								}
+							default:
+								{
+									printf("unknown Cylinder up axis\n");
+								}
+
+							};
+
+
+
+							break;
+						}
+					case CONE_SHAPE_PROXYTYPE:
+						{
+							btConeShapeData* conData = (btConeShapeData*) shapeData;
+							btVector3 halfExtents = implicitShapeDimensions;//+margin;
+							switch (conData->m_upIndex)
+							{
+							case 0:
+								{
+									shape = createConeShapeX(halfExtents.getY(),halfExtents.getX());
+									break;
+								}
+							case 1:
+								{
+									shape = createConeShapeY(halfExtents.getX(),halfExtents.getY());
+									break;
+								}
+							case 2:
+								{
+									shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ());
+									break;
+								}
+							default:
+								{
+									printf("unknown Cone up axis\n");
+								}
+
+							};
+
+
+
+							break;
+						}
+					case MULTI_SPHERE_SHAPE_PROXYTYPE:
+						{
+							btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd;
+							int numSpheres = mss->m_localPositionArraySize;
+
+							btAlignedObjectArray<btVector3> tmpPos;
+							btAlignedObjectArray<btScalar> radii;
+							radii.resize(numSpheres);
+							tmpPos.resize(numSpheres);
+							int i;
+							for ( i=0;i<numSpheres;i++)
+							{
+								tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos);
+								radii[i] = mss->m_localPositionArrayPtr[i].m_radius;
+							}
+							shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres);
+							break;
+						}
+					case CONVEX_HULL_SHAPE_PROXYTYPE:
+						{
+						//	int sz = sizeof(btConvexHullShapeData);
+						//	int sz2 = sizeof(btConvexInternalShapeData);
+						//	int sz3 = sizeof(btCollisionShapeData);
+							btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd;
+							int numPoints = convexData->m_numUnscaledPoints;
+
+							btAlignedObjectArray<btVector3> tmpPoints;
+							tmpPoints.resize(numPoints);
+							int i;
+							for ( i=0;i<numPoints;i++)
+							{
+#ifdef BT_USE_DOUBLE_PRECISION
+							if (convexData->m_unscaledPointsDoublePtr)
+								tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]);
+							if (convexData->m_unscaledPointsFloatPtr)
+								tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]);
+#else
+							if (convexData->m_unscaledPointsFloatPtr)
+								tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]);
+							if (convexData->m_unscaledPointsDoublePtr)
+								tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]);
+#endif //BT_USE_DOUBLE_PRECISION
+							}
+							btConvexHullShape* hullShape = createConvexHullShape();
+							for (i=0;i<numPoints;i++)
+							{
+								hullShape->addPoint(tmpPoints[i]);
+							}
+							hullShape->setMargin(bsd->m_collisionMargin);
+							//hullShape->initializePolyhedralFeatures();
+							shape = hullShape;
+							break;
+						}
+					default:
+						{
+							printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType);
+						}
+				}
+
+				if (shape)
+				{
+					shape->setMargin(bsd->m_collisionMargin);
+
+					btVector3 localScaling;
+					localScaling.deSerializeFloat(bsd->m_localScaling);
+					shape->setLocalScaling(localScaling);
+
+				}
+				break;
+			}
+		case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData;
+			btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&trimesh->m_meshInterface);
+			btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData);
+			if (!meshInterface->getNumSubParts())
+			{
+				return 0;
+			}
+
+			btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling);
+			meshInterface->setScaling(scaling);
+
+
+			btOptimizedBvh* bvh = 0;
+#if 1
+			if (trimesh->m_quantizedFloatBvh)
+			{
+				btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedFloatBvh);
+				if (bvhPtr && *bvhPtr)
+				{
+					bvh = *bvhPtr;
+				} else
+				{
+					bvh = createOptimizedBvh();
+					bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh);
+				}
+			}
+			if (trimesh->m_quantizedDoubleBvh)
+			{
+				btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedDoubleBvh);
+				if (bvhPtr && *bvhPtr)
+				{
+					bvh = *bvhPtr;
+				} else
+				{
+					bvh = createOptimizedBvh();
+					bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh);
+				}
+			}
+#endif
+
+
+			btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh);
+			trimeshShape->setMargin(trimesh->m_collisionMargin);
+			shape = trimeshShape;
+
+			if (trimesh->m_triangleInfoMap)
+			{
+				btTriangleInfoMap* map = createTriangleInfoMap();
+				map->deSerialize(*trimesh->m_triangleInfoMap);
+				trimeshShape->setTriangleInfoMap(map);
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+				gContactAddedCallback = btAdjustInternalEdgeContactsCallback;
+#endif //USE_INTERNAL_EDGE_UTILITY
+
+			}
+
+			//printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin);
+			break;
+		}
+		case COMPOUND_SHAPE_PROXYTYPE:
+			{
+				btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
+				btCompoundShape* compoundShape = createCompoundShape();
+
+				btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0];
+
+
+				btAlignedObjectArray<btCollisionShape*> childShapes;
+				for (int i=0;i<compoundData->m_numChildShapes;i++)
+				{
+					btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i];
+
+					btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape;
+
+					btCollisionShape* childShape = convertCollisionShape(cd);
+					if (childShape)
+					{
+						btTransform localTransform;
+						localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform);
+						compoundShape->addChildShape(localTransform,childShape);
+					} else
+					{
+#ifdef _DEBUG
+						printf("error: couldn't create childShape for compoundShape\n");
+#endif
+					}
+
+				}
+				shape = compoundShape;
+
+				break;
+			}
+		case SOFTBODY_SHAPE_PROXYTYPE:
+			{
+				return 0;
+			}
+		default:
+			{
+#ifdef _DEBUG
+				printf("unsupported shape type (%d)\n",shapeData->m_shapeType);
+#endif
+			}
+		}
+
+		return shape;
+
+}
+
+
+
+char* btCollisionWorldImporter::duplicateName(const char* name)
+{
+	if (name)
+	{
+		int l = (int)strlen(name);
+		char* newName = new char[l+1];
+		memcpy(newName,name,l);
+		newName[l] = 0;
+		m_allocatedNames.push_back(newName);
+		return newName;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData&  meshData)
+{
+	btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer();
+
+	for (int i=0;i<meshData.m_numMeshParts;i++)
+	{
+		btIndexedMesh meshPart;
+		meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles;
+		meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices;
+
+
+		if (meshData.m_meshPartsPtr[i].m_indices32)
+		{
+			meshPart.m_indexType = PHY_INTEGER;
+			meshPart.m_triangleIndexStride = 3*sizeof(int);
+			int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16);
+			m_indexArrays.push_back(indexArray);
+			for (int j=0;j<3*meshPart.m_numTriangles;j++)
+			{
+				indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value;
+			}
+			meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+		} else
+		{
+			if (meshData.m_meshPartsPtr[i].m_3indices16)
+			{
+				meshPart.m_indexType = PHY_SHORT;
+				meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData);
+
+				short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16);
+				m_shortIndexArrays.push_back(indexArray);
+
+				for (int j=0;j<meshPart.m_numTriangles;j++)
+				{
+					indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0];
+					indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1];
+					indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2];
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+			if (meshData.m_meshPartsPtr[i].m_indices16)
+			{
+				meshPart.m_indexType = PHY_SHORT;
+				meshPart.m_triangleIndexStride = 3*sizeof(short int);
+				short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16);
+				m_shortIndexArrays.push_back(indexArray);
+				for (int j=0;j<3*meshPart.m_numTriangles;j++)
+				{
+					indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value;
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+
+			if (meshData.m_meshPartsPtr[i].m_3indices8)
+			{
+				meshPart.m_indexType = PHY_UCHAR;
+				meshPart.m_triangleIndexStride = sizeof(unsigned char)*3;
+
+				unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16);
+				m_charIndexArrays.push_back(indexArray);
+
+				for (int j=0;j<meshPart.m_numTriangles;j++)
+				{
+					indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0];
+					indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1];
+					indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2];
+				}
+
+				meshPart.m_triangleIndexBase = (const unsigned char*)indexArray;
+			}
+		}
+
+		if (meshData.m_meshPartsPtr[i].m_vertices3f)
+		{
+			meshPart.m_vertexType = PHY_FLOAT;
+			meshPart.m_vertexStride = sizeof(btVector3FloatData);
+			btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16);
+			m_floatVertexArrays.push_back(vertices);
+
+			for (int j=0;j<meshPart.m_numVertices;j++)
+			{
+				vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0];
+				vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1];
+				vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[2];
+				vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3];
+			}
+			meshPart.m_vertexBase = (const unsigned char*)vertices;
+		} else
+		{
+			meshPart.m_vertexType = PHY_DOUBLE;
+			meshPart.m_vertexStride = sizeof(btVector3DoubleData);
+
+
+			btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16);
+			m_doubleVertexArrays.push_back(vertices);
+
+			for (int j=0;j<meshPart.m_numVertices;j++)
+			{
+				vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0];
+				vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1];
+				vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[2];
+				vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[3];
+			}
+			meshPart.m_vertexBase = (const unsigned char*)vertices;
+		}
+
+		if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase)
+		{
+			meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType);
+		}
+	}
+
+	return meshInterface;
+}
+
+
+btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData)
+{
+	//create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter
+	btStridingMeshInterfaceData* newData = new btStridingMeshInterfaceData;
+
+	newData->m_scaling = interfaceData->m_scaling;
+	newData->m_numMeshParts = interfaceData->m_numMeshParts;
+	newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts];
+
+	for(int i = 0;i < newData->m_numMeshParts;i++)
+	{
+		btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i];
+		btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i];
+
+		curNewPart->m_numTriangles = curPart->m_numTriangles;
+		curNewPart->m_numVertices = curPart->m_numVertices;
+
+		if(curPart->m_vertices3f)
+		{
+			curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices];
+			memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices);
+		}
+		else
+			curNewPart->m_vertices3f = NULL;
+
+		if(curPart->m_vertices3d)
+		{
+			curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices];
+			memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices);
+		}
+		else
+			curNewPart->m_vertices3d = NULL;
+
+		int numIndices = curNewPart->m_numTriangles * 3;
+		///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time
+		///we catch it by only dealing with m_3indices8 if none of the other indices are initialized
+		bool uninitialized3indices8Workaround =false;
+
+		if(curPart->m_indices32)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_indices32 = new btIntIndexData[numIndices];
+			memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices);
+		}
+		else
+			curNewPart->m_indices32 = NULL;
+
+		if(curPart->m_3indices16)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles];
+			memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles);
+		}
+		else
+			curNewPart->m_3indices16 = NULL;
+
+		if(curPart->m_indices16)
+		{
+			uninitialized3indices8Workaround=true;
+			curNewPart->m_indices16 = new btShortIntIndexData[numIndices];
+			memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices);
+		}
+		else
+			curNewPart->m_indices16 = NULL;
+
+		if(!uninitialized3indices8Workaround && curPart->m_3indices8)
+		{
+			curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles];
+			memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles);
+		}
+		else
+			curNewPart->m_3indices8 = NULL;
+
+	}
+
+	m_allocatedbtStridingMeshInterfaceDatas.push_back(newData);
+
+	return(newData);
+}
+
+#ifdef USE_INTERNAL_EDGE_UTILITY
+extern ContactAddedCallback		gContactAddedCallback;
+
+static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
+{
+
+	btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1);
+		//btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE);
+		//btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED);
+	return true;
+}
+#endif //USE_INTERNAL_EDGE_UTILITY
+
+
+/*
+btRigidBody*  btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName)
+{
+	btVector3 localInertia;
+	localInertia.setZero();
+
+	if (mass)
+		shape->calculateLocalInertia(mass,localInertia);
+
+	btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
+	body->setWorldTransform(startTransform);
+
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->addRigidBody(body);
+
+	if (bodyName)
+	{
+		char* newname = duplicateName(bodyName);
+		m_objectNameMap.insert(body,newname);
+		m_nameBodyMap.insert(newname,body);
+	}
+	m_allocatedRigidBodies.push_back(body);
+	return body;
+
+}
+*/
+
+btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char* name)
+{
+	btCollisionObject** bodyPtr = m_nameColObjMap.find(name);
+	if (bodyPtr && *bodyPtr)
+	{
+		return *bodyPtr;
+	}
+	return 0;
+}
+
+btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName)
+{
+	btCollisionObject* colObj = new btCollisionObject();
+	colObj->setWorldTransform(startTransform);
+	colObj->setCollisionShape(shape);
+	m_collisionWorld->addCollisionObject(colObj);//todo: flags etc
+
+	if (bodyName)
+	{
+		char* newname = duplicateName(bodyName);
+		m_objectNameMap.insert(colObj,newname);
+		m_nameColObjMap.insert(newname,colObj);
+	}
+	m_allocatedCollisionObjects.push_back(colObj);
+
+	return colObj;
+}
+
+
+
+btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
+{
+	btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+btCollisionShape* btCollisionWorldImporter::createBoxShape(const btVector3& halfExtents)
+{
+	btBoxShape* shape = new btBoxShape(halfExtents);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius)
+{
+	btSphereShape* shape = new btSphereShape(radius);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height)
+{
+	btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height)
+{
+	btCapsuleShape* shape = new btCapsuleShape(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height)
+{
+	btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height)
+{
+	btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height)
+{
+	btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height)
+{
+	btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height));
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height)
+{
+	btConeShapeX* shape = new btConeShapeX(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height)
+{
+	btConeShape* shape = new btConeShape(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height)
+{
+	btConeShapeZ* shape = new btConeShapeZ(radius,height);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btTriangleIndexVertexArray*	btCollisionWorldImporter::createTriangleMeshContainer()
+{
+	btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray();
+	m_allocatedTriangleIndexArrays.push_back(in);
+	return in;
+}
+
+btOptimizedBvh*	btCollisionWorldImporter::createOptimizedBvh()
+{
+	btOptimizedBvh* bvh = new btOptimizedBvh();
+	m_allocatedBvhs.push_back(bvh);
+	return bvh;
+}
+
+
+btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap()
+{
+	btTriangleInfoMap* tim = new btTriangleInfoMap();
+	m_allocatedTriangleInfoMaps.push_back(tim);
+	return tim;
+}
+
+btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh)
+{
+	if (bvh)
+	{
+		btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false);
+		bvhTriMesh->setOptimizedBvh(bvh);
+		m_allocatedCollisionShapes.push_back(bvhTriMesh);
+		return bvhTriMesh;
+	}
+
+	btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true);
+	m_allocatedCollisionShapes.push_back(ts);
+	return ts;
+
+}
+btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh)
+{
+	return 0;
+}
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh)
+{
+	btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+
+}
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+
+btConvexHullShape* btCollisionWorldImporter::createConvexHullShape()
+{
+	btConvexHullShape* shape = new btConvexHullShape();
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btCompoundShape* btCollisionWorldImporter::createCompoundShape()
+{
+	btCompoundShape* shape = new btCompoundShape();
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling)
+{
+	btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres)
+{
+	btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
+
+
+	// query for data
+int	btCollisionWorldImporter::getNumCollisionShapes() const
+{
+	return m_allocatedCollisionShapes.size();
+}
+
+btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index)
+{
+	return m_allocatedCollisionShapes[index];
+}
+
+btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name)
+{
+	btCollisionShape** shapePtr = m_nameShapeMap.find(name);
+	if (shapePtr&& *shapePtr)
+	{
+		return *shapePtr;
+	}
+	return 0;
+}
+
+
+const char*	btCollisionWorldImporter::getNameForPointer(const void* ptr) const
+{
+	const char*const * namePtr = m_objectNameMap.find(ptr);
+	if (namePtr && *namePtr)
+		return *namePtr;
+	return 0;
+}
+
+
+int btCollisionWorldImporter::getNumRigidBodies() const
+{
+	return m_allocatedRigidBodies.size();
+}
+
+btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) const
+{
+	return m_allocatedRigidBodies[index];
+}
+
+
+int btCollisionWorldImporter::getNumBvhs() const
+{
+	return m_allocatedBvhs.size();
+}
+ btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const
+{
+	return m_allocatedBvhs[index];
+}
+
+int btCollisionWorldImporter::getNumTriangleInfoMaps() const
+{
+	return m_allocatedTriangleInfoMaps.size();
+}
+
+btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index) const
+{
+	return m_allocatedTriangleInfoMaps[index];
+}
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h

@@ -0,0 +1,190 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_WORLD_IMPORTER_H
+#define BT_COLLISION_WORLD_IMPORTER_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
+
+class btCollisionShape;
+class btCollisionObject;
+struct btBulletSerializedArrays;
+
+
+struct ConstraintInput;
+class btCollisionWorld;
+struct btCollisionShapeData;
+class btTriangleIndexVertexArray;
+class btStridingMeshInterface;
+struct btStridingMeshInterfaceData;
+class btGImpactMeshShape;
+class btOptimizedBvh;
+struct btTriangleInfoMap;
+class btBvhTriangleMeshShape;
+class btPoint2PointConstraint;
+class btHingeConstraint;
+class btConeTwistConstraint;
+class btGeneric6DofConstraint;
+class btGeneric6DofSpringConstraint;
+class btSliderConstraint;
+class btGearConstraint;
+struct btContactSolverInfo;
+
+
+
+
+class btCollisionWorldImporter
+{
+protected:
+	btCollisionWorld* m_collisionWorld;
+
+	int m_verboseMode;
+
+	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
+	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
+
+	btAlignedObjectArray<btOptimizedBvh*>	 m_allocatedBvhs;
+	btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps;
+	btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays;
+	btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas;
+	btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects;
+
+
+	btAlignedObjectArray<char*>				m_allocatedNames;
+
+	btAlignedObjectArray<int*>				m_indexArrays;
+	btAlignedObjectArray<short int*>		m_shortIndexArrays;
+	btAlignedObjectArray<unsigned char*>	m_charIndexArrays;
+
+	btAlignedObjectArray<btVector3FloatData*>	m_floatVertexArrays;
+	btAlignedObjectArray<btVector3DoubleData*>	m_doubleVertexArrays;
+
+
+	btHashMap<btHashPtr,btOptimizedBvh*>	m_bvhMap;
+	btHashMap<btHashPtr,btTriangleInfoMap*>	m_timMap;
+
+	btHashMap<btHashString,btCollisionShape*>	m_nameShapeMap;
+	btHashMap<btHashString,btCollisionObject*>	m_nameColObjMap;
+
+	btHashMap<btHashPtr,const char*>	m_objectNameMap;
+
+	btHashMap<btHashPtr,btCollisionShape*>	m_shapeMap;
+	btHashMap<btHashPtr,btCollisionObject*>	m_bodyMap;
+
+
+	//methods
+
+
+
+	char*	duplicateName(const char* name);
+
+	btCollisionShape* convertCollisionShape(  btCollisionShapeData* shapeData  );
+
+
+public:
+
+	btCollisionWorldImporter(btCollisionWorld* world);
+
+	virtual ~btCollisionWorldImporter();
+
+    bool	convertAllObjects( btBulletSerializedArrays* arrays);
+
+		///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load.
+	///make sure you don't use the dynamics world containing objects after you call this method
+	virtual void deleteAllData();
+
+	void	setVerboseMode(int verboseMode)
+	{
+		m_verboseMode = verboseMode;
+	}
+
+	int getVerboseMode() const
+	{
+		return m_verboseMode;
+	}
+
+		// query for data
+	int	getNumCollisionShapes() const;
+	btCollisionShape* getCollisionShapeByIndex(int index);
+	int getNumRigidBodies() const;
+	btCollisionObject* getRigidBodyByIndex(int index) const;
+	int getNumConstraints() const;
+
+	int getNumBvhs() const;
+	btOptimizedBvh*  getBvhByIndex(int index) const;
+	int getNumTriangleInfoMaps() const;
+	btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const;
+
+	// queris involving named objects
+	btCollisionShape* getCollisionShapeByName(const char* name);
+	btCollisionObject* getCollisionObjectByName(const char* name);
+
+
+	const char*	getNameForPointer(const void* ptr) const;
+
+	///those virtuals are called by load and can be overridden by the user
+
+
+
+	//bodies
+
+	virtual btCollisionObject*  createCollisionObject(	const btTransform& startTransform,	btCollisionShape* shape,const char* bodyName);
+
+	///shapes
+
+	virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant);
+	virtual btCollisionShape* createBoxShape(const btVector3& halfExtents);
+	virtual btCollisionShape* createSphereShape(btScalar radius);
+	virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height);
+	virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height);
+	virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height);
+
+	virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height);
+	virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height);
+	virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height);
+	virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height);
+	virtual class btTriangleIndexVertexArray*	createTriangleMeshContainer();
+	virtual	btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh);
+	virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh);
+#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT
+	virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh);
+#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
+	virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData);
+
+	virtual class btConvexHullShape* createConvexHullShape();
+	virtual class btCompoundShape* createCompoundShape();
+	virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape);
+
+	virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres);
+
+	virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
+
+	///acceleration and connectivity structures
+	virtual btOptimizedBvh*	createOptimizedBvh();
+	virtual btTriangleInfoMap* createTriangleInfoMap();
+
+
+
+
+};
+
+
+#endif //BT_WORLD_IMPORTER_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -0,0 +1,378 @@
+/*
+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"
+#include "btManifoldResult.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btShapePairCallback gCompoundChildShapePairCallback = 0;
+
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
+:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_isSwapped(isSwapped),
+m_sharedManifold(ci.m_manifold)
+{
+	m_ownsManifold = false;
+
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	btAssert (colObjWrap->getCollisionShape()->isCompound());
+	
+	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
+	m_compoundShapeRevision = compoundShape->getUpdateRevision();
+	
+	
+	preallocateChildAlgorithms(body0Wrap,body1Wrap);
+}
+
+void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+{
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+	btAssert (colObjWrap->getCollisionShape()->isCompound());
+	
+	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->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
+		{
+			
+			const btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+			btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
+		}
+	}
+}
+
+void	btCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+	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]);
+		}
+	}
+}
+
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+	removeChildAlgorithms();
+}
+
+
+
+
+struct	btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+	const btCollisionObjectWrapper* m_compoundColObjWrap;
+	const btCollisionObjectWrapper* m_otherObjWrap;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	btCollisionAlgorithm**	m_childCollisionAlgorithms;
+	btPersistentManifold*	m_sharedManifold;
+	
+	btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
+		:m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithms(childCollisionAlgorithms),
+		m_sharedManifold(sharedManifold)
+	{
+
+	}
+
+
+	void	ProcessChildShape(const btCollisionShape* childShape,int index)
+	{
+		btAssert(index>=0);
+		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
+		btAssert(index<compoundShape->getNumChildShapes());
+
+
+		//backup
+		btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
+		
+		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_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
+
+		if (gCompoundChildShapePairCallback)
+		{
+			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
+				return;
+		}
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+
+			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
+
+
+			//the contactpoint is still projected back using the original inverted worldtrans
+			if (!m_childCollisionAlgorithms[index])
+				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
+
+			
+			const btCollisionObjectWrapper* tmpWrap = 0;
+
+			///detect swapping case
+			if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
+			{
+				tmpWrap = m_resultOut->getBody0Wrap();
+				m_resultOut->setBody0Wrap(&compoundWrap);
+				m_resultOut->setShapeIdentifiersA(-1,index);
+			} else
+			{
+				tmpWrap = m_resultOut->getBody1Wrap();
+				m_resultOut->setBody1Wrap(&compoundWrap);
+				m_resultOut->setShapeIdentifiersB(-1,index);
+			}
+
+
+			m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
+
+#if 0
+			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));
+			}
+#endif
+
+			if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
+			{
+				m_resultOut->setBody0Wrap(tmpWrap);
+			} else
+			{
+				m_resultOut->setBody1Wrap(tmpWrap);
+			}
+			
+		}
+	}
+	void		Process(const btDbvtNode* leaf)
+	{
+		int index = leaf->dataAsInt;
+
+		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
+		const btCollisionShape* childShape = compoundShape->getChildShape(index);
+
+#if 0
+		if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+		{
+			btVector3 worldAabbMin,worldAabbMax;
+			btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
+			btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+			m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+		}
+#endif
+
+		ProcessChildShape(childShape,index);
+
+	}
+};
+
+
+
+
+
+
+void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+
+	btAssert (colObjWrap->getCollisionShape()->isCompound());
+	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
+
+	///btCompoundShape might have changed:
+	////make sure the internal child collision algorithm caches are still valid
+	if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
+	{
+		///clear and update all
+		removeChildAlgorithms();
+		
+		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+		m_compoundShapeRevision = compoundShape->getUpdateRevision();
+	}
+
+    if (m_childCollisionAlgorithms.size()==0)
+        return;
+    
+	const btDbvt* tree = compoundShape->getDynamicAabbTree();
+	//use a dynamic aabb tree to cull potential child-overlaps
+	btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
+
+	///we need to refresh all contact manifolds
+	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+	{
+		int i;
+		manifoldArray.resize(0);
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);//??necessary?
+					}
+				}
+				manifoldArray.resize(0);
+			}
+		}
+	}
+
+	if (tree)
+	{
+
+		btVector3 localAabbMin,localAabbMax;
+		btTransform otherInCompoundSpace;
+		otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
+		otherObjWrap->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->collideTVNoStackAlloc(tree->m_root,bounds,stack2,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;
+		manifoldArray.resize(0);
+        const btCollisionShape* childShape = 0;
+        btTransform	orgTrans;
+        
+        btTransform	newChildWorldTrans;
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
+		for (i=0;i<numChildren;i++)
+		{
+			if (m_childCollisionAlgorithms[i])
+			{
+				childShape = compoundShape->getChildShape(i);
+			//if not longer overlapping, remove the algorithm
+				orgTrans = colObjWrap->getWorldTransform();
+                
+				const btTransform& childTrans = compoundShape->getChildTransform(i);
+                newChildWorldTrans = orgTrans*childTrans ;
+
+				//perform an AABB check first
+				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+				otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->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)
+{
+	btAssert(0);
+	//needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
+	btCollisionObject* colObj = m_isSwapped? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+
+	btAssert (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;
+    btTransform	orgTrans;
+    btScalar frac;
+	for (i=0;i<numChildren;i++)
+	{
+		//btCollisionShape* childShape = compoundShape->getChildShape(i);
+
+		//backup
+        orgTrans = colObj->getWorldTransform();
+	
+		const btTransform& childTrans = compoundShape->getChildTransform(i);
+		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		colObj->setWorldTransform( orgTrans*childTrans );
+
+		//btCollisionShape* tmpShape = colObj->getCollisionShape();
+		//colObj->internalSetTemporaryCollisionShape( childShape );
+        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+		if (frac<hitFraction)
+		{
+			hitFraction = frac;
+		}
+		//revert back
+		//colObj->internalSetTemporaryCollisionShape( tmpShape);
+		colObj->setWorldTransform( orgTrans);
+	}
+	return hitFraction;
+
+}
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -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_COMPOUND_COLLISION_ALGORITHM_H
+#define BT_COMPOUND_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+class btDispatcher;
+class btCollisionObject;
+
+class btCollisionShape;
+typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+extern btShapePairCallback gCompoundChildShapePairCallback;
+
+/// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
+class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+	btNodeStack stack2;
+	btManifoldArray manifoldArray;
+
+protected:
+	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
+	bool m_isSwapped;
+
+	class btPersistentManifold*	m_sharedManifold;
+	bool					m_ownsManifold;
+
+
+	int	m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
+	
+	void	removeChildAlgorithms();
+	
+	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+public:
+
+	btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+	virtual ~btCompoundCollisionAlgorithm();
+
+	btCollisionAlgorithm* getChildAlgorithm (int n) const
+	{
+		return m_childCollisionAlgorithms[n];
+	}
+
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+		}
+	};
+
+};
+
+#endif //BT_COMPOUND_COLLISION_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -0,0 +1,428 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 "btCompoundCompoundCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+
+btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
+
+btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
+:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
+{
+
+	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
+	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
+
+	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+
+	const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
+	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+	
+	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+	m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+
+	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+	m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+	
+	
+}
+
+
+btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
+{
+	removeChildAlgorithms();
+	m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
+	btAlignedFree(m_childCollisionAlgorithmCache);
+}
+
+void	btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	int i;
+	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+	for (i=0;i<pairs.size();i++)
+	{
+		if (pairs[i].m_userPointer)
+		{
+			
+			((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
+		}
+	}
+}
+
+
+void	btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+	btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+
+	int numChildren = pairs.size();
+	int i;
+	for (i=0;i<numChildren;i++)
+	{
+		if (pairs[i].m_userPointer)
+		{
+			btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+			algo->~btCollisionAlgorithm();
+			m_dispatcher->freeCollisionAlgorithm(algo);
+		}
+	}
+	m_childCollisionAlgorithmCache->removeAllPairs();
+}
+
+struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
+{
+	int m_numOverlapPairs;
+
+
+	const btCollisionObjectWrapper* m_compound0ColObjWrap;
+	const btCollisionObjectWrapper* m_compound1ColObjWrap;
+	btDispatcher* m_dispatcher;
+	const btDispatcherInfo& m_dispatchInfo;
+	btManifoldResult*	m_resultOut;
+	
+	
+	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
+	
+	btPersistentManifold*	m_sharedManifold;
+	
+	btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
+									const btCollisionObjectWrapper* compound0ObjWrap,
+									btDispatcher* dispatcher,
+									const btDispatcherInfo& dispatchInfo,
+									btManifoldResult*	resultOut,
+									btHashedSimplePairCache* childAlgorithmsCache,
+									btPersistentManifold*	sharedManifold)
+		:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		m_childCollisionAlgorithmCache(childAlgorithmsCache),
+		m_sharedManifold(sharedManifold)
+	{
+
+	}
+
+
+
+	
+	void		Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
+	{
+		BT_PROFILE("btCompoundCompoundLeafCallback::Process");
+		m_numOverlapPairs++;
+
+
+		int childIndex0 = leaf0->dataAsInt;
+		int childIndex1 = leaf1->dataAsInt;
+		
+
+		btAssert(childIndex0>=0);
+		btAssert(childIndex1>=0);
+
+
+		const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
+		btAssert(childIndex0<compoundShape0->getNumChildShapes());
+
+		const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
+		btAssert(childIndex1<compoundShape1->getNumChildShapes());
+
+		const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
+		const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
+
+		//backup
+		btTransform	orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
+		const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
+		btTransform	newChildWorldTrans0 = orgTrans0*childTrans0 ;
+		
+		btTransform	orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
+		const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
+		btTransform	newChildWorldTrans1 = orgTrans1*childTrans1 ;
+		
+
+		//perform an AABB check first
+		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+		childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+		
+		if (gCompoundCompoundChildShapePairCallback)
+		{
+			if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
+				return;
+		}
+
+		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		{
+			btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
+			btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
+			
+
+			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
+
+			btCollisionAlgorithm* colAlgo = 0;
+
+			if (pair)
+			{
+				colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+				
+			} else
+			{
+				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
+				pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
+				btAssert(pair);
+				pair->m_userPointer = colAlgo;
+			}
+
+			btAssert(colAlgo);
+						
+			const btCollisionObjectWrapper* tmpWrap0 = 0;
+			const btCollisionObjectWrapper* tmpWrap1 = 0;
+
+			tmpWrap0 = m_resultOut->getBody0Wrap();
+			tmpWrap1 = m_resultOut->getBody1Wrap();
+
+			m_resultOut->setBody0Wrap(&compoundWrap0);
+			m_resultOut->setBody1Wrap(&compoundWrap1);
+
+			m_resultOut->setShapeIdentifiersA(-1,childIndex0);
+			m_resultOut->setShapeIdentifiersB(-1,childIndex1);
+
+
+			colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
+			
+			m_resultOut->setBody0Wrap(tmpWrap0);
+			m_resultOut->setBody1Wrap(tmpWrap1);
+			
+
+
+		}
+	}
+};
+
+
+static DBVT_INLINE bool		MyIntersect(	const btDbvtAabbMm& a,
+								  const btDbvtAabbMm& b, const btTransform& xform)
+{
+	btVector3 newmin,newmax;
+	btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
+	btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
+	return Intersect(a,newb);
+}
+
+
+static inline void		MycollideTT(	const btDbvtNode* root0,
+								  const btDbvtNode* root1,
+								  const btTransform& xform,
+								  btCompoundCompoundLeafCallback* callback)
+{
+
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=btDbvt::DOUBLE_STACKSIZE-4;
+			btAlignedObjectArray<btDbvt::sStkNN>	stkStack;
+			stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+			stkStack[0]=btDbvt::sStkNN(root0,root1);
+			do	{
+				btDbvt::sStkNN	p=stkStack[--depth];
+				if(MyIntersect(p.a->volume,p.b->volume,xform))
+				{
+					if(depth>treshold)
+					{
+						stkStack.resize(stkStack.size()*2);
+						treshold=stkStack.size()-4;
+					}
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{					
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							callback->Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+	const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
+
+	btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+	btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+
+	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	if (!tree0 || !tree1)
+	{
+		return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
+	}
+	///btCompoundShape might have changed:
+	////make sure the internal child collision algorithm caches are still valid
+	if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
+	{
+		///clear all
+		removeChildAlgorithms();
+		m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+		m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+
+	}
+
+
+	///we need to refresh all contact manifolds
+	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+	///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+	{
+		int i;
+		btManifoldArray manifoldArray;
+		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+		for (i=0;i<pairs.size();i++)
+		{
+			if (pairs[i].m_userPointer)
+			{
+				btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+				algo->getAllContactManifolds(manifoldArray);
+				for (int m=0;m<manifoldArray.size();m++)
+				{
+					if (manifoldArray[m]->getNumContacts())
+					{
+						resultOut->setPersistentManifold(manifoldArray[m]);
+						resultOut->refreshContactPoints();
+						resultOut->setPersistentManifold(0);
+					}
+				}
+				manifoldArray.resize(0);
+			}
+		}
+	}
+
+
+	
+
+	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
+
+
+	const btTransform	xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
+	MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+
+	//printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
+
+	//remove non-overlapping child pairs
+
+	{
+		btAssert(m_removePairs.size()==0);
+
+		//iterate over all children, perform an AABB check inside ProcessChildShape
+		btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+		
+		int i;
+		btManifoldArray	manifoldArray;
+        
+		
+
+        
+        
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
+		for (i=0;i<pairs.size();i++)
+		{
+			if (pairs[i].m_userPointer)
+			{
+				btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
+
+				{
+					btTransform	orgTrans0;
+					const btCollisionShape* childShape0 = 0;
+					
+					btTransform	newChildWorldTrans0;
+					btTransform	orgInterpolationTrans0;
+					childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
+					orgTrans0 = col0ObjWrap->getWorldTransform();
+					orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
+					const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
+					newChildWorldTrans0 = orgTrans0*childTrans0 ;
+					childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+				}
+
+				{
+					btTransform	orgInterpolationTrans1;
+					const btCollisionShape* childShape1 = 0;
+					btTransform	orgTrans1;
+					btTransform	newChildWorldTrans1;
+
+					childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
+					orgTrans1 = col1ObjWrap->getWorldTransform();
+					orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
+					const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
+					newChildWorldTrans1 = orgTrans1*childTrans1 ;
+					childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+				}
+				
+				
+
+				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				{
+					algo->~btCollisionAlgorithm();
+					m_dispatcher->freeCollisionAlgorithm(algo);
+					m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
+				}
+			}
+		}
+		for (int i=0;i<m_removePairs.size();i++)
+		{
+			m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
+		}
+		m_removePairs.clear();
+	}
+
+}
+
+btScalar	btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	btAssert(0);
+	return 0.f;
+
+}
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h

@@ -0,0 +1,89 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+
+#include "btCompoundCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.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 "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h"
+class btDispatcher;
+class btCollisionObject;
+
+class btCollisionShape;
+typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
+
+/// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
+class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm
+{
+
+	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
+	btSimplePairArray m_removePairs;
+
+
+	int	m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
+	int	m_compoundShapeRevision1;
+	
+	void	removeChildAlgorithms();
+	
+//	void	preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+public:
+
+	btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+	virtual ~btCompoundCompoundCollisionAlgorithm();
+
+	
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+			return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+		}
+	};
+
+};
+
+#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp

@@ -0,0 +1,242 @@
+/*
+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 "btConvex2dConvex2dAlgorithm.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/CollisionShapes/btCapsuleShape.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/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+}
+
+btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_simplexSolver(simplexSolver),
+m_pdSolver(pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false)
+{
+	(void)body0Wrap;
+	(void)body1Wrap;
+}
+
+
+
+
+btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
+
+
+	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
+	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
+
+	btVector3  normalOnB;
+	btVector3  pointOnBWorld;
+
+	{
+
+
+		btGjkPairDetector::ClosestPointInput input;
+
+		btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+		//TODO: if (dispatchInfo.m_useContinuous)
+		gjkPairDetector.setMinkowskiA(min0);
+		gjkPairDetector.setMinkowskiB(min1);
+
+		{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+			input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+		}
+
+		input.m_transformA = body0Wrap->getWorldTransform();
+		input.m_transformB = body1Wrap->getWorldTransform();
+
+		gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+		btVector3 v0,v1;
+		btVector3 sepNormalWorldSpace;
+
+	}
+
+	if (m_ownManifold)
+	{
+		resultOut->refreshContactPoints();
+	}
+
+}
+
+
+
+
+btScalar	btConvex2dConvex2dAlgorithm::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;
+
+
+	//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;
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h

@@ -0,0 +1,92 @@
+/*
+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_2D_CONVEX_2D_ALGORITHM_H
+#define BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+
+class btConvexPenetrationDepthSolver;
+
+
+///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape
+///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation
+class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm
+{
+	btSimplexSolverInterface*		m_simplexSolver;
+	btConvexPenetrationDepthSolver* m_pdSolver;
+
+	
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+public:
+
+	btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+
+	virtual ~btConvex2dConvex2dAlgorithm();
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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;
+		int m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm));
+			return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+		}
+	};
+
+
+};
+
+#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -0,0 +1,337 @@
+/*
+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 "LinearMath/btQuickprof.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"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_isSwapped(isSwapped),
+m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
+{
+}
+
+btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
+{
+}
+
+void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	if (m_btConvexTriangleCallback.m_manifoldPtr)
+	{
+		manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
+	}
+}
+
+
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
+	  m_dispatcher(dispatcher),
+	m_dispatchInfoPtr(0)
+{
+	m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
+	m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
+	
+	  //
+	  // create the manifold from the dispatcher 'manifold pool'
+	  //
+	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
+
+  	  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)
+{
+	BT_PROFILE("btConvexTriangleCallback::processTriangle");
+
+	if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
+	{
+		return;
+	}
+
+        //just for debugging purposes
+        //printf("triangle %d",m_triangleCount++);
+
+
+
+	btCollisionAlgorithmConstructionInfo ci;
+	ci.m_dispatcher1 = m_dispatcher;
+
+
+
+#if 0	
+	
+	///debug drawing of the overlapping triangles
+	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
+	{
+		const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
+		btVector3 color(1,1,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);
+	}
+#endif
+	
+	if (m_convexBodyWrap->getCollisionShape()->isConvex())
+	{
+		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		tm.setMargin(m_collisionMarginTriangle);
+		
+		
+		btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
+
+		const btCollisionObjectWrapper* tmpWrap = 0;
+
+		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
+		{
+			tmpWrap = m_resultOut->getBody0Wrap();
+			m_resultOut->setBody0Wrap(&triObWrap);
+			m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+		}
+		else
+		{
+			tmpWrap = m_resultOut->getBody1Wrap();
+			m_resultOut->setBody1Wrap(&triObWrap);
+			m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+		}
+	
+		colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
+
+		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
+		{
+			m_resultOut->setBody0Wrap(tmpWrap);
+		} else
+		{
+			m_resultOut->setBody1Wrap(tmpWrap);
+		}
+		
+
+
+		colAlgo->~btCollisionAlgorithm();
+		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
+	}
+
+}
+
+
+
+void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
+{
+	m_convexBodyWrap = convexBodyWrap;
+	m_triBodyWrap = triBodyWrap;
+
+	m_dispatchInfoPtr = &dispatchInfo;
+	m_collisionMarginTriangle = collisionMarginTriangle;
+	m_resultOut = resultOut;
+
+	//recalc aabbs
+	btTransform convexInTriangleSpace;
+	convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
+	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
+	
+	const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
+
+	if (triBodyWrap->getCollisionShape()->isConcave())
+	{
+
+
+		
+		const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
+		
+		if (convexBodyWrap->getCollisionShape()->isConvex())
+		{
+			btScalar collisionMarginTriangle = concaveShape->getMargin();
+					
+			resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+			m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
+
+			m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
+
+			concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
+			
+			resultOut->refreshContactPoints();
+
+			m_btConvexTriangleCallback.clearWrapperData();
+	
+		}
+	
+	}
+
+}
+
+
+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)
+		{
+			BT_PROFILE("processTriangle");
+			(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.);
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h

@@ -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.
+*/
+
+#ifndef BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+#define BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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.
+ATTRIBUTE_ALIGNED16(class)  btConvexTriangleCallback : public btTriangleCallback
+{
+
+	btVector3	m_aabbMin;
+	btVector3	m_aabbMax ;
+
+	const btCollisionObjectWrapper* m_convexBodyWrap;
+	const btCollisionObjectWrapper* m_triBodyWrap;
+
+
+
+	btManifoldResult* m_resultOut;
+	btDispatcher*	m_dispatcher;
+	const btDispatcherInfo* m_dispatchInfoPtr;
+	btScalar m_collisionMarginTriangle;
+	
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+int	m_triangleCount;
+	
+	btPersistentManifold*	m_manifoldPtr;
+
+	btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+	void	setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut);
+
+	void	clearWrapperData()
+	{
+		m_convexBodyWrap = 0;
+		m_triBodyWrap = 0;
+	}
+	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.
+ATTRIBUTE_ALIGNED16(class)  btConvexConcaveCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+
+	btConvexTriangleCallback m_btConvexTriangleCallback;
+
+	bool	m_isSwapped;
+
+
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+	virtual ~btConvexConcaveCollisionAlgorithm();
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+		}
+	};
+
+	struct SwappedCreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm));
+			return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+		}
+	};
+
+};
+
+#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -0,0 +1,786 @@
+/*
+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.
+*/
+
+///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
+///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
+///with reproduction case
+//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+//#define ZERO_MARGIN
+
+#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/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.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/btGjkEpa2.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+///////////
+
+
+
+static SIMD_FORCE_INLINE void segmentsClosestPoints(
+	btVector3& ptsVector,
+	btVector3& offsetA,
+	btVector3& offsetB,
+	btScalar& tA, btScalar& tB,
+	const btVector3& translation,
+	const btVector3& dirA, btScalar hlenA,
+	const btVector3& dirB, btScalar hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	btScalar dirA_dot_dirB = btDot(dirA,dirB);
+	btScalar dirA_dot_trans = btDot(dirA,translation);
+	btScalar dirB_dot_trans = btDot(dirB,translation);
+
+	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
+
+
+static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
+	btVector3& normalOnB,
+	btVector3& pointOnB,
+	btScalar capsuleLengthA,
+	btScalar	capsuleRadiusA,
+	btScalar capsuleLengthB,
+	btScalar	capsuleRadiusB,
+	int capsuleAxisA,
+	int capsuleAxisB,
+	const btTransform& transformA,
+	const btTransform& transformB,
+	btScalar distanceThreshold )
+{
+	btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
+	btVector3 translationA = transformA.getOrigin();
+	btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
+	btVector3 translationB = transformB.getOrigin();
+
+	// translation between centers
+
+	btVector3 translation = translationB - translationA;
+
+	// compute the closest points of the capsule line segments
+
+	btVector3 ptsVector;           // the vector between the closest points
+	
+	btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
+	btScalar tA, tB;              // parameters on line segment
+
+	segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+						   directionA, capsuleLengthA, directionB, capsuleLengthB );
+
+	btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
+
+	if ( distance > distanceThreshold )
+		return distance;
+
+	btScalar lenSqr = ptsVector.length2();
+	if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
+		btVector3 q;
+		btPlaneSpace1(directionA,normalOnB,q);
+	} else
+	{
+		// compute the contact normal
+		normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+	}
+	pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+
+	return distance;
+}
+
+
+
+
+
+
+
+//////////
+
+
+
+
+
+btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+{
+	m_numPerturbationIterations = 0;
+	m_minimumPointsPerturbationThreshold = 3;
+	m_simplexSolver = simplexSolver;
+	m_pdSolver = pdSolver;
+}
+
+btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
+{ 
+}
+
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_simplexSolver(simplexSolver),
+m_pdSolver(pdSolver),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false),
+#ifdef USE_SEPDISTANCE_UTIL2
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
+#endif
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+	(void)body0Wrap;
+	(void)body1Wrap;
+}
+
+
+
+
+btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+
+struct btPerturbedContactResult : public btManifoldResult
+{
+	btManifoldResult* m_originalManifoldResult;
+	btTransform m_transformA;
+	btTransform m_transformB;
+	btTransform	m_unPerturbedTransform;
+	bool	m_perturbA;
+	btIDebugDraw*	m_debugDrawer;
+
+
+	btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
+		:m_originalManifoldResult(originalResult),
+		m_transformA(transformA),
+		m_transformB(transformB),
+		m_unPerturbedTransform(unPerturbedTransform),
+		m_perturbA(perturbA),
+		m_debugDrawer(debugDrawer)
+	{
+	}
+	virtual ~ btPerturbedContactResult()
+	{
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
+	{
+		btVector3 endPt,startPt;
+		btScalar newDepth;
+		btVector3 newNormal;
+
+		if (m_perturbA)
+		{
+			btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
+			endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
+			newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
+			startPt = endPt+normalOnBInWorld*newDepth;
+		} else
+		{
+			endPt = pointInWorld + normalOnBInWorld*orgDepth;
+			startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
+			newDepth = (endPt -  startPt).dot(normalOnBInWorld);
+			
+		}
+
+//#define DEBUG_CONTACTS 1
+#ifdef DEBUG_CONTACTS
+		m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
+		m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
+		m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
+#endif //DEBUG_CONTACTS
+
+		
+		m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
+	}
+
+};
+
+extern btScalar gContactBreakingThreshold;
+
+
+//
+// Convex-Convex collision algorithm
+//
+void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+	if (!m_manifoldPtr)
+	{
+		//swapped?
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
+	
+
+	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
+	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
+
+	btVector3  normalOnB;
+		btVector3  pointOnBWorld;
+#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+	{
+		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+	//	btVector3 localScalingA = capsuleA->getLocalScaling();
+	//	btVector3 localScalingB = capsuleB->getLocalScaling();
+		
+		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+			capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
+			body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
+
+		if (dist<threshold)
+		{
+			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
+		}
+		resultOut->refreshContactPoints();
+		return;
+	}
+#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+
+
+
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+	}
+
+	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
+#endif //USE_SEPDISTANCE_UTIL2
+
+	{
+
+	
+	btGjkPairDetector::ClosestPointInput input;
+
+	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
+	//TODO: if (dispatchInfo.m_useContinuous)
+	gjkPairDetector.setMinkowskiA(min0);
+	gjkPairDetector.setMinkowskiB(min1);
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
+	} else
+#endif //USE_SEPDISTANCE_UTIL2
+	{
+		//if (dispatchInfo.m_convexMaxDistanceUseCPT)
+		//{
+		//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+		//} else
+		//{
+		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+//		}
+
+		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	}
+
+	input.m_transformA = body0Wrap->getWorldTransform();
+	input.m_transformB = body1Wrap->getWorldTransform();
+
+
+
+	
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	btScalar sepDist = 0.f;
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+	{
+		sepDist = gjkPairDetector.getCachedSeparatingDistance();
+		if (sepDist>SIMD_EPSILON)
+		{
+			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+			//now perturbe directions to get multiple contact points
+			
+		}
+	}
+#endif //USE_SEPDISTANCE_UTIL2
+
+	if (min0->isPolyhedral() && min1->isPolyhedral())
+	{
+
+
+		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
+		{
+			virtual void setShapeIdentifiersA(int partId0,int index0){}
+			virtual void setShapeIdentifiersB(int partId1,int index1){}
+			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
+			{
+			}
+		};
+
+		
+		struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
+		{
+			btDiscreteCollisionDetectorInterface::Result* m_originalResult;
+			btVector3	m_reportedNormalOnWorld;
+			btScalar m_marginOnA;
+			btScalar m_marginOnB;
+			btScalar	m_reportedDistance;
+			
+			bool		m_foundResult;
+			btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
+			:m_originalResult(result),
+			m_marginOnA(marginOnA),
+			m_marginOnB(marginOnB),
+			m_foundResult(false)
+			{
+			}
+			
+			virtual void setShapeIdentifiersA(int partId0,int index0){}
+			virtual void setShapeIdentifiersB(int partId1,int index1){}
+			virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) 
+			{
+				m_reportedDistance = depthOrg;
+				m_reportedNormalOnWorld = normalOnBInWorld;
+				
+				btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
+				m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
+				if (m_reportedDistance<0.f)
+				{
+					m_foundResult = true;					
+				}
+				m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
+			}
+		};
+
+		
+		btDummyResult dummy;
+
+///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
+
+		btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
+		btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
+
+		btWithoutMarginResult	withoutMargin(resultOut, min0Margin,min1Margin);
+
+		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
+		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
+		if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
+		{
+
+
+			
+
+			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+			btScalar minDist = -1e30f;
+			btVector3 sepNormalWorldSpace;
+			bool foundSepAxis  = true;
+
+			if (dispatchInfo.m_enableSatConvex)
+			{
+				foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
+					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+					body0Wrap->getWorldTransform(), 
+					body1Wrap->getWorldTransform(),
+					sepNormalWorldSpace,*resultOut);
+			} else
+			{
+#ifdef ZERO_MARGIN
+				gjkPairDetector.setIgnoreMargin(true);
+				gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+#else
+
+
+				gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
+				//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+#endif //ZERO_MARGIN
+				//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+				//if (l2>SIMD_EPSILON)
+				{
+					sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+					//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
+					minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
+	
+#ifdef ZERO_MARGIN
+					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
+#else
+					foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
+#endif
+				}
+			}
+			if (foundSepAxis)
+			{
+				
+//				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
+
+				worldVertsB1.resize(0);
+				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+					body0Wrap->getWorldTransform(), 
+																 body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
+																 *resultOut);
+ 				
+			}
+			if (m_ownManifold)
+			{
+				resultOut->refreshContactPoints();
+			}
+			return;
+
+		} else
+		{
+			//we can also deal with convex versus triangle (without connectivity data)
+			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
+			{
+
+				btVertexArray vertices;
+				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
+				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
+				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
+				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
+				
+				//tri->initializePolyhedralFeatures();
+
+				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+				btVector3 sepNormalWorldSpace;
+				btScalar minDist =-1e30f;
+				btScalar maxDist = threshold;
+				
+				bool foundSepAxis = false;
+				if (0)
+				{
+					polyhedronB->initializePolyhedralFeatures();
+					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
+					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
+					body0Wrap->getWorldTransform(), 
+					body1Wrap->getWorldTransform(),
+					sepNormalWorldSpace,*resultOut);
+				//	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
+
+				} else
+				{
+#ifdef ZERO_MARGIN
+					gjkPairDetector.setIgnoreMargin(true);
+					gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+#else
+					gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+#endif//ZERO_MARGIN
+					
+					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+					if (l2>SIMD_EPSILON)
+					{
+						sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+						//minDist = gjkPairDetector.getCachedSeparatingDistance();
+						//maxDist = threshold;
+						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
+						foundSepAxis = true;
+					}
+				}
+
+				
+			if (foundSepAxis)
+			{
+				worldVertsB2.resize(0);
+				btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
+					body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
+			}
+				
+				
+				if (m_ownManifold)
+				{
+					resultOut->refreshContactPoints();
+				}
+				
+				return;
+			}
+			
+		}
+
+
+	}
+	
+	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
+
+	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+	
+	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
+	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+	{
+		
+		int i;
+		btVector3 v0,v1;
+		btVector3 sepNormalWorldSpace;
+		btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+	
+		if (l2>SIMD_EPSILON)
+		{
+			sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+			
+			btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+
+
+			bool perturbeA = true;
+			const btScalar angleLimit = 0.125f * SIMD_PI;
+			btScalar perturbeAngle;
+			btScalar radiusA = min0->getAngularMotionDisc();
+			btScalar radiusB = min1->getAngularMotionDisc();
+			if (radiusA < radiusB)
+			{
+				perturbeAngle = gContactBreakingThreshold /radiusA;
+				perturbeA = true;
+			} else
+			{
+				perturbeAngle = gContactBreakingThreshold / radiusB;
+				perturbeA = false;
+			}
+			if ( perturbeAngle > angleLimit ) 
+					perturbeAngle = angleLimit;
+
+			btTransform unPerturbedTransform;
+			if (perturbeA)
+			{
+				unPerturbedTransform = input.m_transformA;
+			} else
+			{
+				unPerturbedTransform = input.m_transformB;
+			}
+			
+			for ( i=0;i<m_numPerturbationIterations;i++)
+			{
+				if (v0.length2()>SIMD_EPSILON)
+				{
+				btQuaternion perturbeRot(v0,perturbeAngle);
+				btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+				btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
+				
+				
+				if (perturbeA)
+				{
+					input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
+					input.m_transformB = body1Wrap->getWorldTransform();
+	#ifdef DEBUG_CONTACTS
+					dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
+	#endif //DEBUG_CONTACTS
+				} else
+				{
+					input.m_transformA = body0Wrap->getWorldTransform();
+					input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
+	#ifdef DEBUG_CONTACTS
+					dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
+	#endif
+				}
+				
+				btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
+				gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+				}
+			}
+		}
+	}
+
+	
+
+#ifdef USE_SEPDISTANCE_UTIL2
+	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
+	{
+		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
+	}
+#endif //USE_SEPDISTANCE_UTIL2
+
+
+	}
+
+	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;
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -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 BT_CONVEX_CONVEX_ALGORITHM_H
+#define BT_CONVEX_CONVEX_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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"
+#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
+
+class btConvexPenetrationDepthSolver;
+
+///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise
+///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions.
+///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
+///for certain pairs that have a small size ratio
+
+//#define USE_SEPDISTANCE_UTIL2 1
+
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
+class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
+{
+#ifdef USE_SEPDISTANCE_UTIL2
+	btConvexSeparatingDistanceUtil	m_sepDistance;
+#endif
+	btSimplexSolverInterface*		m_simplexSolver;
+	btConvexPenetrationDepthSolver* m_pdSolver;
+
+	btVertexArray worldVertsB1;
+	btVertexArray worldVertsB2;
+	
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool			m_lowLevelOfDetail;
+	
+	int m_numPerturbationIterations;
+	int m_minimumPointsPerturbationThreshold;
+
+
+	///cache separating vector to speedup collision detection
+	
+
+public:
+
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
+	virtual ~btConvexConvexAlgorithm();
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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;
+		int m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+
+		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+		
+		virtual ~CreateFunc();
+
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+		}
+	};
+
+
+};
+
+#endif //BT_CONVEX_CONVEX_ALGORITHM_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -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.
+*/
+
+#include "btConvexPlaneCollisionAlgorithm.h"
+
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+//#include <stdio.h>
+
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
+: btCollisionAlgorithm(ci),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped),
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
+{
+	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap;
+
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+
+btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+    const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+
+    bool hasCollision = false;
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	const btScalar& planeConstant = planeShape->getPlaneConstant();
+	
+	btTransform convexWorldTransform = convexObjWrap->getWorldTransform();
+	btTransform convexInPlaneTrans;
+	convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform;
+	//now perturbe the convex-world transform
+	convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
+	btTransform planeInConvex;
+	planeInConvex= convexWorldTransform.inverse() * planeObjWrap->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 = planeObjWrap->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 = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
+		btVector3 pOnB = vtxInPlaneWorld;
+		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+	}
+}
+
+
+void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	if (!m_manifoldPtr)
+		return;
+
+	const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap;
+
+	btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape();
+	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape();
+
+	bool hasCollision = false;
+	const btVector3& planeNormal = planeShape->getPlaneNormal();
+	const btScalar& planeConstant = planeShape->getPlaneConstant();
+	btTransform planeInConvex;
+	planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform();
+	btTransform convexInPlaneTrans;
+	convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->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 = planeObjWrap->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 = planeObjWrap->getWorldTransform().getBasis() * planeNormal;
+		btVector3 pOnB = vtxInPlaneWorld;
+		resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
+	}
+
+	//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
+	//they keep on rolling forever because of the additional off-center contact points
+	//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
+	if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+	{
+		btVector3 v0,v1;
+		btPlaneSpace1(planeNormal,v0,v1);
+		//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+		const btScalar angleLimit = 0.125f * SIMD_PI;
+		btScalar perturbeAngle;
+		btScalar radius = convexShape->getAngularMotionDisc();
+		perturbeAngle = gContactBreakingThreshold / radius;
+		if ( perturbeAngle > angleLimit ) 
+				perturbeAngle = angleLimit;
+
+		btQuaternion perturbeRot(v0,perturbeAngle);
+		for (int i=0;i<m_numPerturbationIterations;i++)
+		{
+			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+			btQuaternion rotq(planeNormal,iterationAngle);
+			collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut);
+		}
+	}
+
+	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.);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

@@ -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 BT_CONVEX_PLANE_COLLISION_ALGORITHM_H
+#define BT_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;
+	int			m_numPerturbationIterations;
+	int			m_minimumPointsPerturbationThreshold;
+
+public:
+
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
+
+	virtual ~btConvexPlaneCollisionAlgorithm();
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+	void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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
+	{
+		int	m_numPerturbationIterations;
+		int m_minimumPointsPerturbationThreshold;
+			
+		CreateFunc() 
+			: m_numPerturbationIterations(1),
+			m_minimumPointsPerturbationThreshold(0)
+		{
+		}
+		
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			} else
+			{
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
+			}
+		}
+	};
+
+};
+
+#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -0,0 +1,308 @@
+/*
+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/btCompoundCompoundCollisionAlgorithm.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/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();
+
+	if (constructionInfo.m_useEpaPenetrationAlgorithm)
+	{
+		mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+		m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+	}else
+	{
+		mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+		m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+	}
+	
+	//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(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
+	m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::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(btCompoundCompoundCollisionAlgorithm);
+
+	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
+		
+	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);
+	}
+	
+	collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0;
+	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_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_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(m_compoundCompoundCreateFunc);
+
+	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) && btBroadphaseProxy::isCompound(proxyType1))
+	{
+		return m_compoundCompoundCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	} else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+}
+
+void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+{
+	btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
+	convexConvex->m_numPerturbationIterations = numPerturbationIterations;
+	convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+}
+
+void	btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+{
+	btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF;
+	cpCF->m_numPerturbationIterations = numPerturbationIterations;
+	cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+	
+	btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF;
+	pcCF->m_numPerturbationIterations = numPerturbationIterations;
+	pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -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.
+*/
+
+#ifndef BT_DEFAULT_COLLISION_CONFIGURATION
+#define BT_DEFAULT_COLLISION_CONFIGURATION
+
+#include "btCollisionConfiguration.h"
+class btVoronoiSimplexSolver;
+class btConvexPenetrationDepthSolver;
+
+struct	btDefaultCollisionConstructionInfo
+{
+	btPoolAllocator*	m_persistentManifoldPool;
+	btPoolAllocator*	m_collisionAlgorithmPool;
+	int					m_defaultMaxPersistentManifoldPoolSize;
+	int					m_defaultMaxCollisionAlgorithmPoolSize;
+	int					m_customCollisionAlgorithmMaxElementSize;
+	int					m_useEpaPenetrationAlgorithm;
+
+	btDefaultCollisionConstructionInfo()
+		:m_persistentManifoldPool(0),
+		m_collisionAlgorithmPool(0),
+		m_defaultMaxPersistentManifoldPoolSize(4096),
+		m_defaultMaxCollisionAlgorithmPoolSize(4096),
+		m_customCollisionAlgorithmMaxElementSize(0),
+		m_useEpaPenetrationAlgorithm(true)
+	{
+	}
+};
+
+
+
+///btCollisionConfiguration allows to configure Bullet collision detection
+///stack allocator, pool memory allocators
+///@todo: describe the meaning
+class	btDefaultCollisionConfiguration : public btCollisionConfiguration
+{
+
+protected:
+
+	int	m_persistentManifoldPoolSize;
+	
+
+	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_compoundCompoundCreateFunc;
+	
+	btCollisionAlgorithmCreateFunc*	m_swappedCompoundCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
+	btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+	btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
+	btCollisionAlgorithmCreateFunc* m_boxSphereCF;
+
+	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	btVoronoiSimplexSolver*	getSimplexSolver()
+	{
+		return m_simplexSolver;
+	}
+
+
+	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+
+	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
+	///By default, this feature is disabled for best performance.
+	///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
+	///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
+	///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
+	///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
+	///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
+	void	setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
+
+	void	setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
+
+};
+
+#endif //BT_DEFAULT_COLLISION_CONFIGURATION
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp

@@ -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 (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+}
+
+btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* )
+{
+	return btScalar(1.);
+}
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h

@@ -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 BT_EMPTY_ALGORITH
+#define BT_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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+        {
+			(void)body0Wrap;
+			(void)body1Wrap;
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm));
+			return new(mem) btEmptyAlgorithm(ci);
+		}
+	};
+
+} ATTRIBUTE_ALIGNED(16);
+
+#endif //BT_EMPTY_ALGORITH

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btGhostObject.cpp

@@ -0,0 +1,171 @@
+/*
+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 "btGhostObject.h"
+#include "btCollisionWorld.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "LinearMath/btAabbUtil2.h"
+
+btGhostObject::btGhostObject()
+{
+	m_internalType = CO_GHOST_OBJECT;
+}
+
+btGhostObject::~btGhostObject()
+{
+	///btGhostObject should have been removed from the world, so no overlapping objects
+	btAssert(!m_overlappingObjects.size());
+}
+
+
+void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index==m_overlappingObjects.size())
+	{
+		//not found
+		m_overlappingObjects.push_back(otherObject);
+	}
+}
+
+void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index<m_overlappingObjects.size())
+	{
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects.pop_back();
+	}
+}
+
+
+btPairCachingGhostObject::btPairCachingGhostObject()
+{
+	m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+}
+
+btPairCachingGhostObject::~btPairCachingGhostObject()
+{
+	m_hashPairCache->~btHashedOverlappingPairCache();
+	btAlignedFree( m_hashPairCache );
+}
+
+void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
+{
+	btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
+	btAssert(actualThisProxy);
+
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index==m_overlappingObjects.size())
+	{
+		m_overlappingObjects.push_back(otherObject);
+		m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy);
+	}
+}
+
+void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1)
+{
+	btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
+	btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle();
+	btAssert(actualThisProxy);
+
+	btAssert(otherObject);
+	int index = m_overlappingObjects.findLinearSearch(otherObject);
+	if (index<m_overlappingObjects.size())
+	{
+		m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
+		m_overlappingObjects.pop_back();
+		m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher);
+	}
+}
+
+
+void	btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) 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_overlappingObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_overlappingObjects[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))
+			{
+				btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						resultCallback,
+						allowedCcdPenetration);
+			}
+		}
+	}
+
+}
+
+void	btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
+{
+	btTransform rayFromTrans;
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFromWorld);
+	btTransform  rayToTrans;
+	rayToTrans.setIdentity();
+	rayToTrans.setOrigin(rayToWorld);
+
+
+	int i;
+	for (i=0;i<m_overlappingObjects.size();i++)
+	{
+		btCollisionObject*	collisionObject= m_overlappingObjects[i];
+		//only perform raycast if filterMask matches
+		if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
+							collisionObject,
+								collisionObject->getCollisionShape(),
+								collisionObject->getWorldTransform(),
+								resultCallback);
+		}
+	}
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btGhostObject.h

@@ -0,0 +1,175 @@
+/*
+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.
+*/
+
+#ifndef BT_GHOST_OBJECT_H
+#define BT_GHOST_OBJECT_H
+
+
+#include "btCollisionObject.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "btCollisionWorld.h"
+
+class btConvexShape;
+
+class btDispatcher;
+
+///The btGhostObject can keep track of all objects that are overlapping
+///By default, this overlap is based on the AABB
+///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
+///We plan on adding rayTest and other queries for the btGhostObject
+ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
+{
+protected:
+
+	btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
+
+public:
+
+	btGhostObject();
+
+	virtual ~btGhostObject();
+
+	void	convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const;
+
+	void	rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; 
+
+	///this method is mainly for expert/internal use only.
+	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+	///this method is mainly for expert/internal use only.
+	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+	int	getNumOverlappingObjects() const
+	{
+		return m_overlappingObjects.size();
+	}
+
+	btCollisionObject*	getOverlappingObject(int index)
+	{
+		return m_overlappingObjects[index];
+	}
+
+	const btCollisionObject*	getOverlappingObject(int index) const
+	{
+		return m_overlappingObjects[index];
+	}
+
+	btAlignedObjectArray<btCollisionObject*>&	getOverlappingPairs()
+	{
+		return m_overlappingObjects;
+	}
+
+	const btAlignedObjectArray<btCollisionObject*>	getOverlappingPairs() const
+	{
+		return m_overlappingObjects;
+	}
+
+	//
+	// internal cast
+	//
+
+	static const btGhostObject*	upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+			return (const btGhostObject*)colObj;
+		return 0;
+	}
+	static btGhostObject*			upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()==CO_GHOST_OBJECT)
+			return (btGhostObject*)colObj;
+		return 0;
+	}
+
+};
+
+class	btPairCachingGhostObject : public btGhostObject
+{
+	btHashedOverlappingPairCache*	m_hashPairCache;
+
+public:
+
+	btPairCachingGhostObject();
+
+	virtual ~btPairCachingGhostObject();
+
+	///this method is mainly for expert/internal use only.
+	virtual void	addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
+
+	virtual void	removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
+
+	btHashedOverlappingPairCache*	getOverlappingPairCache()
+	{
+		return m_hashPairCache;
+	}
+
+};
+
+
+
+///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
+class btGhostPairCallback : public btOverlappingPairCallback
+{
+	
+public:
+	btGhostPairCallback()
+	{
+	}
+
+	virtual ~btGhostPairCallback()
+	{
+		
+	}
+
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		if (ghost0)
+			ghost0->addOverlappingObjectInternal(proxy1, proxy0);
+		if (ghost1)
+			ghost1->addOverlappingObjectInternal(proxy0, proxy1);
+		return 0;
+	}
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	{
+		btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
+		btGhostObject* ghost0 = 		btGhostObject::upcast(colObj0);
+		btGhostObject* ghost1 = 		btGhostObject::upcast(colObj1);
+		if (ghost0)
+			ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
+		if (ghost1)
+			ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
+		return 0;
+	}
+
+	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
+	{
+		btAssert(0);
+		//need to keep track of all ghost objects and call them here
+		//m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
+	}
+
+	
+
+};
+
+#endif
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp

@@ -0,0 +1,276 @@
+/*
+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 "btHashedSimplePairCache.h"
+
+
+#include <stdio.h>
+
+int	gOverlappingSimplePairs = 0;
+int gRemoveSimplePairs =0;
+int gAddedSimplePairs =0;
+int gFindSimplePairs =0;
+
+
+
+
+btHashedSimplePairCache::btHashedSimplePairCache() {
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+
+btHashedSimplePairCache::~btHashedSimplePairCache()
+{
+}
+
+
+
+
+
+
+void btHashedSimplePairCache::removeAllPairs()
+{
+	m_overlappingPairArray.clear();
+	m_hashTable.clear();
+	m_next.clear();
+
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
+{
+	gFindSimplePairs++;
+	
+	
+	/*if (indexA > indexB) 
+		btSwap(indexA, indexB);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	if (hash >= m_hashTable.size())
+	{
+		return NULL;
+	}
+
+	int index = m_hashTable[hash];
+	while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == BT_SIMPLE_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	btAssert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void	btHashedSimplePairCache::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_SIMPLE_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = BT_SIMPLE_NULL_PAIR;
+		}
+
+		for(i=0;i<curHashtableSize;i++)
+		{
+	
+			const btSimplePair& pair = m_overlappingPairArray[i];
+			int indexA = pair.m_indexA;
+			int indexB = pair.m_indexB;
+			
+			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
+{
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+
+
+	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+
+	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
+	void* mem = &m_overlappingPairArray.expandNonInitializing();
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+	}
+	
+	pair = new (mem) btSimplePair(indexA,indexB);
+
+	pair->m_userPointer = 0;
+	
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = count;
+
+	return pair;
+}
+
+
+
+void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
+{
+	gRemoveSimplePairs++;
+	
+
+	/*if (indexA > indexB) 
+		btSwap(indexA, indexB);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	
+	void* userData = pair->m_userPointer;
+
+
+	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_SIMPLE_NULL_PAIR);
+
+	int previous = BT_SIMPLE_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_SIMPLE_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 btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
+		/* missing swap here too, Nat. */ 
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1));
+
+	index = m_hashTable[lastHash];
+	btAssert(index != BT_SIMPLE_NULL_PAIR);
+
+	previous = BT_SIMPLE_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != BT_SIMPLE_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>
+
+
+
+
+
+
+
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h

@@ -0,0 +1,172 @@
+/*
+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_HASHED_SIMPLE_PAIR_CACHE_H
+#define BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+const int BT_SIMPLE_NULL_PAIR=0xffffffff;
+
+struct btSimplePair
+{
+	btSimplePair(int indexA,int indexB)
+		:m_indexA(indexA),
+		m_indexB(indexB),
+		m_userPointer(0)
+	{
+	}
+
+	int m_indexA;
+	int m_indexB;
+	union
+	{
+		void*	m_userPointer;
+		int		m_userValue;
+	};
+};
+
+typedef btAlignedObjectArray<btSimplePair>	btSimplePairArray;
+
+
+
+extern int gOverlappingSimplePairs;
+extern int gRemoveSimplePairs;
+extern int gAddedSimplePairs;
+extern int gFindSimplePairs;
+
+
+
+
+class btHashedSimplePairCache
+{
+	btSimplePairArray	m_overlappingPairArray;
+		
+
+protected:
+	
+	btAlignedObjectArray<int>	m_hashTable;
+	btAlignedObjectArray<int>	m_next;
+	
+
+public:
+	btHashedSimplePairCache();
+	virtual ~btHashedSimplePairCache();
+	
+	void removeAllPairs();
+
+	virtual void*	removeOverlappingPair(int indexA,int indexB);
+	
+	// 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 btSimplePair* 	addOverlappingPair(int indexA,int indexB)
+	{
+		gAddedSimplePairs++;
+
+		return internalAddPair(indexA,indexB);
+	}
+
+	
+	virtual btSimplePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const btSimplePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	btSimplePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const btSimplePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	
+	btSimplePair* findPair(int indexA,int indexB);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	
+	btSimplePair* 	internalAddPair(int indexA, int indexB);
+
+	void	growTables();
+
+	SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB)
+	{	
+		return pair.m_indexA == indexA && pair.m_indexB == indexB;
+	}
+
+	
+	
+	SIMD_FORCE_INLINE	unsigned int getHash(unsigned int indexA, unsigned int indexB)
+	{
+		int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<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 btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash)
+	{
+		
+		int index = m_hashTable[hash];
+		
+		while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == BT_SIMPLE_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		btAssert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	
+};
+
+
+
+
+#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

@@ -0,0 +1,838 @@
+#include "btInternalEdgeUtility.h"
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+//#define DEBUG_INTERNAL_EDGE
+
+#ifdef DEBUG_INTERNAL_EDGE
+#include <stdio.h>
+#endif //DEBUG_INTERNAL_EDGE
+
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+static btIDebugDraw* gDebugDrawer = 0;
+
+void	btSetDebugDrawer(btIDebugDraw* debugDrawer)
+{
+	gDebugDrawer = debugDrawer;
+}
+
+static void    btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color)
+{
+	if (gDebugDrawer)
+		gDebugDrawer->drawLine(from,to,color);
+}
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+static int	btGetHash(int partId, int triangleIndex)
+{
+	int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
+	return hash;
+}
+
+
+
+static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
+{
+	const btVector3 refAxis0  = edgeA;
+	const btVector3 refAxis1  = normalA;
+	const btVector3 swingAxis = normalB;
+	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	return  angle;
+}
+
+
+struct btConnectivityProcessor : public btTriangleCallback
+{
+	int				m_partIdA;
+	int				m_triangleIndexA;
+	btVector3*		m_triangleVerticesA;
+	btTriangleInfoMap*	m_triangleInfoMap;
+
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		//skip self-collisions
+		if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
+			return;
+
+		//skip duplicates (disabled for now)
+		//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
+		//	return;
+
+		//search for shared vertices and edges
+		int numshared = 0;
+		int sharedVertsA[3]={-1,-1,-1};
+		int sharedVertsB[3]={-1,-1,-1};
+
+		///skip degenerate triangles
+		btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2();
+		if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold)
+			return;
+
+
+		btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2();
+		///skip degenerate triangles
+		if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold)
+			return;
+
+#if 0
+		printf("triangle A[0]	=	(%f,%f,%f)\ntriangle A[1]	=	(%f,%f,%f)\ntriangle A[2]	=	(%f,%f,%f)\n",
+			m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(),
+			m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(),
+			m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ());
+
+		printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex);
+		printf("triangle B[0]	=	(%f,%f,%f)\ntriangle B[1]	=	(%f,%f,%f)\ntriangle B[2]	=	(%f,%f,%f)\n",
+			triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(),
+			triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(),
+			triangle[2].getX(),triangle[2].getY(),triangle[2].getZ());
+#endif
+
+		for (int i=0;i<3;i++)
+		{
+			for (int j=0;j<3;j++)
+			{
+				if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold)
+				{
+					sharedVertsA[numshared] = i;
+					sharedVertsB[numshared] = j;
+					numshared++;
+					///degenerate case
+					if(numshared >= 3)
+						return;
+				}
+			}
+			///degenerate case
+			if(numshared >= 3)
+				return;
+		}
+		switch (numshared)
+		{
+		case 0:
+			{
+				break;
+			}
+		case 1:
+			{
+				//shared vertex
+				break;
+			}
+		case 2:
+			{
+				//shared edge
+				//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
+				if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
+				{
+					sharedVertsA[0] = 2;
+					sharedVertsA[1] = 0;
+					int tmp = sharedVertsB[1];
+					sharedVertsB[1] = sharedVertsB[0];
+					sharedVertsB[0] = tmp;
+				}
+
+				int hash = btGetHash(m_partIdA,m_triangleIndexA);
+
+				btTriangleInfo* info = m_triangleInfoMap->find(hash);
+				if (!info)
+				{
+					btTriangleInfo tmp;
+					m_triangleInfoMap->insert(hash,tmp);
+					info = m_triangleInfoMap->find(hash);
+				}
+
+				int sumvertsA = sharedVertsA[0]+sharedVertsA[1];
+				int otherIndexA = 3-sumvertsA;
+
+				
+				btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]);
+
+				btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]);
+				int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]);
+
+				btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]);
+				//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
+
+				btVector3 normalA;
+				btVector3 normalB;
+				tA.calcNormal(normalA);
+				tB.calcNormal(normalB);
+				edge.normalize();
+				btVector3 edgeCrossA = edge.cross(normalA).normalize();
+
+				{
+					btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]];
+					if (edgeCrossA.dot(tmp) < 0)
+					{
+						edgeCrossA*=-1;
+					}
+				}
+
+				btVector3 edgeCrossB = edge.cross(normalB).normalize();
+
+				{
+					btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]];
+					if (edgeCrossB.dot(tmp) < 0)
+					{
+						edgeCrossB*=-1;
+					}
+				}
+
+				btScalar	angle2 = 0;
+				btScalar	ang4 = 0.f;
+
+
+				btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB);
+				btScalar len2 = calculatedEdge.length2();
+
+				btScalar correctedAngle(0);
+				//btVector3 calculatedNormalB = normalA;
+				bool isConvex = false;
+
+				if (len2<m_triangleInfoMap->m_planarEpsilon)
+				{
+					angle2 = 0.f;
+					ang4 = 0.f;
+				} else
+				{
+
+					calculatedEdge.normalize();
+					btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA);
+					calculatedNormalA.normalize();
+					angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB);
+					ang4 = SIMD_PI-angle2;
+					btScalar dotA = normalA.dot(edgeCrossB);
+					///@todo: check if we need some epsilon, due to floating point imprecision
+					isConvex = (dotA<0.);
+
+					correctedAngle = isConvex ? ang4 : -ang4;
+				}
+
+				
+
+				
+							
+				//alternatively use 
+				//btVector3 calculatedNormalB2 = quatRotate(orn,normalA);
+
+
+				switch (sumvertsA)
+				{
+				case 1:
+					{
+						btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						btScalar bla = computedNormalB.dot(normalB);
+						if (bla<0)
+						{
+							computedNormalB*=-1;
+							info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB;
+						}
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif//DEBUG_INTERNAL_EDGE
+
+						info->m_edgeV0V1Angle = -correctedAngle;
+
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V0V1_CONVEX;
+						break;
+					}
+				case 2:
+					{
+						btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						if (computedNormalB.dot(normalB)<0)
+						{
+							computedNormalB*=-1;
+							info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB;
+						}
+
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif //DEBUG_INTERNAL_EDGE
+						info->m_edgeV2V0Angle = -correctedAngle;
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V2V0_CONVEX;
+						break;	
+					}
+				case 3:
+					{
+						btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2];
+						btQuaternion orn(edge,-correctedAngle);
+						btVector3 computedNormalB = quatRotate(orn,normalA);
+						if (computedNormalB.dot(normalB)<0)
+						{
+							info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB;
+							computedNormalB*=-1;
+						}
+#ifdef DEBUG_INTERNAL_EDGE
+						if ((computedNormalB-normalB).length()>0.0001)
+						{
+							printf("warning: normals not identical\n");
+						}
+#endif //DEBUG_INTERNAL_EDGE
+						info->m_edgeV1V2Angle = -correctedAngle;
+
+						if (isConvex)
+							info->m_flags |= TRI_INFO_V1V2_CONVEX;
+						break;
+					}
+				}
+
+				break;
+			}
+		default:
+			{
+				//				printf("warning: duplicate triangle\n");
+			}
+
+		}
+	}
+};
+/////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////
+
+void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
+{
+	//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
+	if (trimeshShape->getTriangleInfoMap())
+		return;
+
+	trimeshShape->setTriangleInfoMap(triangleInfoMap);
+
+	btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
+	const btVector3& meshScaling = meshInterface->getScaling();
+
+	for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
+	{
+		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;
+		//PHY_ScalarType indexType=0;
+
+		btVector3 triangleVerts[3];
+		meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,	type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
+		btVector3 aabbMin,aabbMax;
+
+		for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
+		{
+			unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
+
+			for (int j=2;j>=0;j--)
+			{
+
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+				if (type == PHY_FLOAT)
+				{
+					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					triangleVerts[j] = btVector3(
+						graphicsbase[0]*meshScaling.getX(),
+						graphicsbase[1]*meshScaling.getY(),
+						graphicsbase[2]*meshScaling.getZ());
+				}
+				else
+				{
+					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+					triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
+				}
+			}
+			aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+			aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
+			aabbMin.setMin(triangleVerts[0]);
+			aabbMax.setMax(triangleVerts[0]);
+			aabbMin.setMin(triangleVerts[1]);
+			aabbMax.setMax(triangleVerts[1]);
+			aabbMin.setMin(triangleVerts[2]);
+			aabbMax.setMax(triangleVerts[2]);
+
+			btConnectivityProcessor connectivityProcessor;
+			connectivityProcessor.m_partIdA = partId;
+			connectivityProcessor.m_triangleIndexA = triangleIndex;
+			connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
+			connectivityProcessor.m_triangleInfoMap  = triangleInfoMap;
+
+			trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
+		}
+
+	}
+
+}
+
+
+
+
+// Given a point and a line segment (defined by two points), compute the closest point
+// in the line.  Cap the point at the endpoints of the line segment.
+void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
+{
+	btVector3 lineDelta     = line1 - line0;
+
+	// Handle degenerate lines
+	if ( lineDelta.fuzzyZero())
+	{
+		nearestPoint = line0;
+	}
+	else
+	{
+		btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta);
+
+		// Clamp the point to conform to the segment's endpoints
+		if ( delta < 0 )
+			delta = 0;
+		else if ( delta > 1 )
+			delta = 1;
+
+		nearestPoint = line0 + lineDelta*delta;
+	}
+}
+
+
+
+
+bool	btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal)
+{
+	btVector3 tri_normal = tri_normal_org;
+	//we only have a local triangle normal, not a local contact normal -> only normal in world space...
+	//either compute the current angle all in local space, or all in world space
+
+	btVector3 edgeCross = edge.cross(tri_normal).normalize();
+	btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB);
+
+	if (correctedEdgeAngle<0)
+	{
+		if (curAngle < correctedEdgeAngle)
+		{
+			btScalar diffAngle = correctedEdgeAngle-curAngle;
+			btQuaternion rotation(edge,diffAngle );
+			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			return true;
+		}
+	}
+
+	if (correctedEdgeAngle>=0)
+	{
+		if (curAngle > correctedEdgeAngle)
+		{
+			btScalar diffAngle = correctedEdgeAngle-curAngle;
+			btQuaternion rotation(edge,diffAngle );
+			clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB;
+			return true;
+		}
+	}
+	return false;
+}
+
+
+
+/// Changes a btManifoldPoint collision normal to the normal from the mesh.
+void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
+{
+	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
+	if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
+		return;
+
+	btBvhTriangleMeshShape* trimesh = 0;
+	
+	if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
+	   trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
+   else	   
+	   trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
+	   
+   	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
+	if (!triangleInfoMapPtr)
+		return;
+
+	int hash = btGetHash(partId0,index0);
+
+
+	btTriangleInfo* info = triangleInfoMapPtr->find(hash);
+	if (!info)
+		return;
+
+	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
+	
+	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
+	btVector3 v0,v1,v2;
+	tri_shape->getVertex(0,v0);
+	tri_shape->getVertex(1,v1);
+	tri_shape->getVertex(2,v2);
+
+	//btVector3 center = (v0+v1+v2)*btScalar(1./3.);
+
+	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
+	btVector3 tri_normal;
+	tri_shape->calcNormal(tri_normal);
+
+	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
+	btVector3 nearest;
+	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
+
+	btVector3 contact = cp.m_localPointB;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	const btTransform& tr = colObj0->getWorldTransform();
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+
+	bool isNearEdge = false;
+
+	int numConcaveEdgeHits = 0;
+	int numConvexEdgeHits = 0;
+
+	btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+	localContactNormalOnB.normalize();//is this necessary?
+	
+	// Get closest edge
+	int      bestedge=-1;
+	btScalar    disttobestedge=BT_LARGE_FLOAT;
+	//
+	// Edge 0 -> 1
+	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{	
+	   btVector3 nearest;
+	   btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
+	   btScalar     len=(contact-nearest).length();
+	   //
+	   if( len < disttobestedge )
+	   {
+	      bestedge=0;
+	      disttobestedge=len;
+      }	      
+   }	   
+	// Edge 1 -> 2
+	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{	
+	   btVector3 nearest;
+	   btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
+	   btScalar     len=(contact-nearest).length();
+	   //
+	   if( len < disttobestedge )
+	   {
+	      bestedge=1;
+	      disttobestedge=len;
+      }	      
+   }	   
+	// Edge 2 -> 0
+	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{	
+	   btVector3 nearest;
+	   btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
+	   btScalar     len=(contact-nearest).length();
+	   //
+	   if( len < disttobestedge )
+	   {
+	      bestedge=2;
+	      disttobestedge=len;
+      }	      
+   }   	      	
+	
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+   btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
+   btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
+#endif   
+	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		if( bestedge==0 )
+		{
+			btVector3 edge(v0-v1);
+			isNearEdge = true;
+
+			if (info->m_edgeV0V1Angle==btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+
+				btQuaternion orn(edge,info->m_edgeV0V1Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal. (what about cp.m_distance1?)
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+							
+						}
+					}
+				}
+			}
+		}
+	}
+
+	btNearestPointInLineSegment(contact,v1,v2,nearest);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+   btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
+#endif   
+
+	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		if( bestedge==1 )
+		{
+			isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+			btVector3 edge(v1-v2);
+
+			isNearEdge = true;
+
+			if (info->m_edgeV1V2Angle == btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+				
+				btQuaternion orn(edge,info->m_edgeV1V2Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal.
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	btNearestPointInLineSegment(contact,v2,v0,nearest);
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+   btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
+#endif   
+
+	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
+	{
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+		btScalar len = (contact-nearest).length();
+		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
+		if( bestedge==2 )
+		{
+			isNearEdge = true;
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+			btVector3 edge(v2-v0);
+
+			if (info->m_edgeV2V0Angle==btScalar(0))
+			{
+				numConcaveEdgeHits++;
+			} else
+			{
+
+				bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
+				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
+	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
+	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+				btVector3 nA = swapFactor * tri_normal;
+				btQuaternion orn(edge,info->m_edgeV2V0Angle);
+				btVector3 computedNormalB = quatRotate(orn,tri_normal);
+				if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
+					computedNormalB*=-1;
+				btVector3 nB = swapFactor*computedNormalB;
+
+#ifdef DEBUG_INTERNAL_EDGE
+				{
+					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
+				}
+#endif //DEBUG_INTERNAL_EDGE
+
+				btScalar	NdotA = localContactNormalOnB.dot(nA);
+				btScalar	NdotB = localContactNormalOnB.dot(nB);
+				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
+
+				if (backFacingNormal)
+				{
+					numConcaveEdgeHits++;
+				}
+				else
+				{
+					numConvexEdgeHits++;
+					//				printf("hitting convex edge\n");
+
+
+					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
+					btVector3 clampedLocalNormal;
+					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
+					if (isClamped)
+					{
+						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
+						{
+							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
+							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
+							cp.m_normalWorldOnB = newNormal;
+							// Reproject collision point along normal.
+							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+						}
+					}
+				} 
+			}
+			
+
+		}
+	}
+
+#ifdef DEBUG_INTERNAL_EDGE
+	{
+		btVector3 color(0,1,1);
+		btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
+	}
+#endif //DEBUG_INTERNAL_EDGE
+
+	if (isNearEdge)
+	{
+
+		if (numConcaveEdgeHits>0)
+		{
+			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
+			{
+				//fix tri_normal so it pointing the same direction as the current local contact normal
+				if (tri_normal.dot(localContactNormalOnB) < 0)
+				{
+					tri_normal *= -1;
+				}
+				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
+			} else
+			{
+				btVector3 newNormal = tri_normal *frontFacing;
+				//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
+				btScalar d = newNormal.dot(localContactNormalOnB) ;
+				if (d< 0)
+				{
+					return;
+				}
+				//modify the normal to be the triangle normal (or backfacing normal)
+				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
+			}
+						
+			// Reproject collision point along normal.
+			cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
+			cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
+		}
+	}
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h

@@ -0,0 +1,47 @@
+
+#ifndef BT_INTERNAL_EDGE_UTILITY_H
+#define BT_INTERNAL_EDGE_UTILITY_H
+
+#include "LinearMath/btHashMap.h"
+#include "LinearMath/btVector3.h"
+
+#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h"
+
+///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges.
+///See also http://code.google.com/p/bullet/issues/detail?id=27
+
+class btBvhTriangleMeshShape;
+class btCollisionObject;
+struct btCollisionObjectWrapper;
+class btManifoldPoint;
+class btIDebugDraw;
+
+
+
+enum btInternalEdgeAdjustFlags
+{
+	BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1,
+	BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended
+	BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4
+};
+
+
+///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
+void	btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap);
+
+
+///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
+///If this info map is missing, or the triangle is not store in this map, nothing will be done
+void	btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);
+
+///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly.
+///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap
+//#define BT_INTERNAL_EDGE_DEBUG_DRAW
+
+#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
+void	btSetDebugDrawer(btIDebugDraw* debugDrawer);
+#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
+
+
+#endif //BT_INTERNAL_EDGE_UTILITY_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -0,0 +1,190 @@
+/*
+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"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback		gContactAddedCallback=0;
+
+
+
+btScalar	btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+btScalar	btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
+    
+    const btScalar MAX_FRICTION  = btScalar(10.);
+    if (friction < -MAX_FRICTION)
+        friction = -MAX_FRICTION;
+    if (friction > MAX_FRICTION)
+        friction = MAX_FRICTION;
+    return friction;
+}
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+btScalar	btManifoldResult::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;
+
+}
+
+btScalar	btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	return body0->getRestitution() * body1->getRestitution();
+}
+
+btScalar	btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    return body0->getContactDamping() + body1->getContactDamping();
+}
+
+btScalar	btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    
+    btScalar s0 = body0->getContactStiffness();
+    btScalar s1 = body1->getContactStiffness();
+    
+    btScalar tmp0 = btScalar(1)/s0;
+    btScalar tmp1 = btScalar(1)/s1;
+    btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
+    return combinedStiffness;
+}
+
+
+btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		:m_manifoldPtr(0),
+		m_body0Wrap(body0Wrap),
+		m_body1Wrap(body1Wrap)
+#ifdef DEBUG_PART_INDEX
+		,m_partId0(-1),
+	m_partId1(-1),
+	m_index0(-1),
+	m_index1(-1)
+#endif //DEBUG_PART_INDEX
+{
+}
+
+
+void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+{
+	btAssert(m_manifoldPtr);
+	//order in manifold needs to match
+
+	if (depth > m_manifoldPtr->getContactBreakingThreshold())
+//	if (depth > m_manifoldPtr->getContactProcessingThreshold())
+		return;
+
+	bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+
+	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+
+	btVector3 localA;
+	btVector3 localB;
+	
+	if (isSwapped)
+	{
+		localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+		localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
+	} else
+	{
+		localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+		localB = m_body1Wrap->getCollisionObject()->getWorldTransform().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_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+	newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+    newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+	
+	if (    (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
+            (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
+    {
+        newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
+    }
+	
+	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
+	
+
+	
+   //BP mod, store contact triangles.
+	if (isSwapped)
+	{
+		newPt.m_partId0 = m_partId1;
+		newPt.m_partId1 = m_partId0;
+		newPt.m_index0  = m_index1;
+		newPt.m_index1  = m_index0;
+	} else
+	{
+		newPt.m_partId0 = m_partId0;
+		newPt.m_partId1 = m_partId1;
+		newPt.m_index0  = m_index0;
+		newPt.m_index1  = m_index1;
+	}
+	//printf("depth=%f\n",depth);
+	///@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_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+		   (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+	{
+		//experimental feature info, for per-triangle material etc.
+		const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
+		const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
+		(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
+	}
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -0,0 +1,154 @@
+/*
+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_MANIFOLD_RESULT_H
+#define BT_MANIFOLD_RESULT_H
+
+class btCollisionObject;
+struct btCollisionObjectWrapper;
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1);
+extern ContactAddedCallback		gContactAddedCallback;
+
+//#define DEBUG_PART_INDEX 1
+
+
+///btManifoldResult is a helper class to manage  contact results.
+class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
+{
+protected:
+
+	btPersistentManifold* m_manifoldPtr;
+
+	const btCollisionObjectWrapper* m_body0Wrap;
+	const btCollisionObjectWrapper* m_body1Wrap;
+	int	m_partId0;
+	int m_partId1;
+	int m_index0;
+	int m_index1;
+	
+
+public:
+
+	btManifoldResult()
+#ifdef DEBUG_PART_INDEX
+		:
+	m_partId0(-1),
+	m_partId1(-1),
+	m_index0(-1),
+	m_index1(-1)
+#endif //DEBUG_PART_INDEX
+	{
+	}
+
+	btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+	virtual ~btManifoldResult() {};
+
+	void	setPersistentManifold(btPersistentManifold* manifoldPtr)
+	{
+		m_manifoldPtr = manifoldPtr;
+	}
+
+	const btPersistentManifold*	getPersistentManifold() const
+	{
+		return m_manifoldPtr;
+	}
+	btPersistentManifold*	getPersistentManifold()
+	{
+		return m_manifoldPtr;
+	}
+
+	virtual void setShapeIdentifiersA(int partId0,int index0)
+	{
+		m_partId0=partId0;
+		m_index0=index0;
+	}
+
+	virtual void setShapeIdentifiersB(	int partId1,int index1)
+	{
+		m_partId1=partId1;
+		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_body0Wrap->getCollisionObject();
+
+		if (isSwapped)
+		{
+			m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform());
+		} else
+		{
+			m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform());
+		}
+	}
+
+	const btCollisionObjectWrapper* getBody0Wrap() const
+	{
+		return m_body0Wrap;
+	}
+	const btCollisionObjectWrapper* getBody1Wrap() const
+	{
+		return m_body1Wrap;
+	}
+
+	void setBody0Wrap(const btCollisionObjectWrapper* obj0Wrap)
+	{
+		m_body0Wrap = obj0Wrap;
+	}
+
+	void setBody1Wrap(const btCollisionObjectWrapper* obj1Wrap)
+	{
+		m_body1Wrap = obj1Wrap;
+	}
+
+	const btCollisionObject* getBody0Internal() const
+	{
+		return m_body0Wrap->getCollisionObject();
+	}
+
+	const btCollisionObject* getBody1Internal() const
+	{
+		return m_body1Wrap->getCollisionObject();
+	}
+
+	/// in the future we can let the user override the methods to combine restitution and friction
+	static btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
+};
+
+#endif //BT_MANIFOLD_RESULT_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -0,0 +1,450 @@
+
+/*
+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():
+m_splitIslands(true)
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+		m_unionFind.reset(n);
+}
+		
+
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+{
+	
+	{
+		btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
+		const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
+		if (numOverlappingPairs)
+		{
+		btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
+		
+		for (int i=0;i<numOverlappingPairs;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());
+			}
+		}
+		}
+	}
+}
+
+#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+	// 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];
+			//Adding filtering here
+			if (!collisionObject->isStaticOrKinematicObject())
+			{
+				collisionObject->setIslandTag(index++);
+			}
+			collisionObject->setCompanionId(-1);
+			collisionObject->setHitFraction(btScalar(1.));
+		}
+	}
+	// do the union find
+
+	initUnionFind( index );
+
+	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) );
+				//Set the correct object offset in Collision Object Array
+				m_unionFind.getElement(index).m_sz = i;
+				collisionObject->setCompanionId(-1);
+				index++;
+			} else
+			{
+				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
+			}
+		}
+	}
+}
+
+
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+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++;
+		}
+	}
+}
+
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+
+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 ) const
+		{
+			return getIslandId(lhs) < getIslandId(rhs);
+		}
+};
+
+
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
+{
+
+	BT_PROFILE("islandUnionFindAndQuickSort");
+	
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	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");
+			}
+
+			btAssert((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");
+				}
+
+				btAssert((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");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+
+	
+	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);
+		 
+		 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+		 const btCollisionObject* colObj1 = static_cast<const 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)
+			{
+				if (colObj0->hasContactResponse())
+					colObj1->activate();
+			}
+			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				if (colObj1->hasContactResponse())
+					colObj0->activate();
+			}
+			if(m_splitIslands)
+			{ 
+				//filtering for response
+				if (dispatcher->needsResponse(colObj0,colObj1))
+					m_islandmanifold.push_back(manifold);
+			}
+		}
+	}
+}
+
+
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher,collisionWorld);
+
+	int endIslandIndex=1;
+	int startIslandIndex;
+	int numElem = getUnionFind().getNumElements();
+
+	BT_PROFILE("processIslands");
+
+	if(!m_splitIslands)
+	{
+		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+		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());
+
+		//tried a radix sort, but quicksort/heapsort seems still faster
+		//@todo rewrite island management
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+		//m_islandmanifold.heapSort(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 = true;
+	                
+					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 = false;
+					}
+	                
+
+			//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);
+		}
+	} // else if(!splitIslands) 
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -0,0 +1,81 @@
+/*
+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_SIMULATION_ISLAND_MANAGER_H
+#define BT_SIMULATION_ISLAND_MANAGER_H
+
+#include "BulletCollision/CollisionDispatch/btUnionFind.h"
+#include "btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btCollisionObject.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;
+	
+	bool m_splitIslands;
+	
+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,btCollisionWorld* collisionWorld, IslandCallback* callback);
+
+	void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+
+	bool getSplitIslands()
+	{
+		return m_splitIslands;
+	}
+	void setSplitIslands(bool doSplitIslands)
+	{
+		m_splitIslands = doSplitIslands;
+	}
+
+};
+
+#endif //BT_SIMULATION_ISLAND_MANAGER_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp

@@ -0,0 +1,214 @@
+/*
+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 "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+//#include <stdio.h>
+
+btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
+: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_isSwapped(isSwapped)
+{
+	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap;
+	
+	if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()))
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+
+btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+
+
+void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+	(void)resultOut;
+	if (!m_manifoldPtr)
+		return;
+
+	const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+
+	btVector3 pOnBox;
+
+	btVector3 normalOnSurfaceB;
+	btScalar penetrationDepth;
+	btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
+	const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
+	btScalar radius = sphere0->getRadius();
+	btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
+	{
+		/// report a contact. internally this will be kept persistent, and contact reduction is done
+		resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
+	}
+
+	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.);
+}
+
+
+bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
+{
+	const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
+	btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
+	btScalar boxMargin = boxShape->getMargin();
+	penetrationDepth = 1.0f;
+
+	// convert the sphere position to the box's local space
+	btTransform const &m44T = boxObjWrap->getWorldTransform();
+	btVector3 sphereRelPos = m44T.invXform(sphereCenter);
+
+	// Determine the closest point to the sphere center in the box
+	btVector3 closestPoint = sphereRelPos;
+	closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
+	closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
+	closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
+	closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
+	closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
+	closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
+	
+	btScalar intersectionDist = fRadius + boxMargin;
+	btScalar contactDist = intersectionDist + maxContactDistance;
+	normal = sphereRelPos - closestPoint;
+
+	//if there is no penetration, we are done
+	btScalar dist2 = normal.length2();
+	if (dist2 > contactDist * contactDist)
+	{
+		return false;
+	}
+
+	btScalar distance;
+
+	//special case if the sphere center is inside the box
+	if (dist2 <= SIMD_EPSILON)
+	{
+		distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
+	}
+	else //compute the penetration details
+	{
+		distance = normal.length();
+		normal /= distance;
+	}
+
+	pointOnBox = closestPoint + normal * boxMargin;
+//	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
+	penetrationDepth = distance - intersectionDist;
+
+	// transform back in world space
+	btVector3 tmp = m44T(pointOnBox);
+	pointOnBox = tmp;
+//	tmp = m44T(v3PointOnSphere);
+//	v3PointOnSphere = tmp;
+	tmp = m44T.getBasis() * normal;
+	normal = tmp;
+
+	return true;
+}
+
+btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) 
+{
+	//project the center of the sphere on the closest face of the box
+	btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
+	btScalar minDist = faceDist;
+	closestPoint.setX( boxHalfExtent.getX() );
+	normal.setValue(btScalar(1.0f),  btScalar(0.0f),  btScalar(0.0f));
+
+	faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setX( -boxHalfExtent.getX() );
+		normal.setValue(btScalar(-1.0f),  btScalar(0.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setY( boxHalfExtent.getY() );
+		normal.setValue(btScalar(0.0f),  btScalar(1.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setY( -boxHalfExtent.getY() );
+		normal.setValue(btScalar(0.0f),  btScalar(-1.0f),  btScalar(0.0f));
+	}
+
+	faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setZ( boxHalfExtent.getZ() );
+		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(1.0f));
+	}
+
+	faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
+	if (faceDist < minDist)
+	{
+		minDist = faceDist;
+		closestPoint = sphereRelPos;
+		closestPoint.setZ( -boxHalfExtent.getZ() );
+		normal.setValue(btScalar(0.0f),  btScalar(0.0f),  btScalar(-1.0f));
+	}
+
+	return minDist;
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h

@@ -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 BT_SPHERE_BOX_COLLISION_ALGORITHM_H
+#define BT_SPHERE_BOX_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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 btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_isSwapped;
+	
+public:
+
+	btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped);
+
+	virtual ~btSphereBoxCollisionAlgorithm();
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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);
+		}
+	}
+
+	bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance );
+
+	btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal );
+	
+	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
+	{
+		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
+			if (!m_swapped)
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false);
+			} else
+			{
+				return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true);
+			}
+		}
+	};
+
+};
+
+#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -0,0 +1,106 @@
+/*
+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"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
+: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
+m_ownManifold(false),
+m_manifoldPtr(mf)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	(void)dispatchInfo;
+
+	if (!m_manifoldPtr)
+		return;
+
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape();
+	btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape();
+
+	btVector3 diff = col0Wrap->getWorldTransform().getOrigin()-  col1Wrap->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 = col1Wrap->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.);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h

@@ -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 BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
+#define BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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 btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	
+public:
+	btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap);
+
+	btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap)
+		{
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
+			return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap);
+		}
+	};
+
+};
+
+#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -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"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_ownManifold(false),
+m_manifoldPtr(mf),
+m_swapped(swapped)
+{
+	if (!m_manifoldPtr)
+	{
+		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
+		m_ownManifold = true;
+	}
+}
+
+btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+	if (!m_manifoldPtr)
+		return;
+
+	const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap;
+	const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap;
+
+	btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape();
+	btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape();
+	
+	/// report a contact. internally this will be kept persistent, and contact reduction is done
+	resultOut->setPersistentManifold(m_manifoldPtr);
+	SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
+	
+	btDiscreteCollisionDetectorInterface::ClosestPointInput input;
+	input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
+	input.m_transformA = sphereObjWrap->getWorldTransform();
+	input.m_transformB = triObjWrap->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.);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

@@ -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_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+#define BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+
+#include "btActivatingCollisionAlgorithm.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 btActivatingCollisionAlgorithm
+{
+	bool	m_ownManifold;
+	btPersistentManifold*	m_manifoldPtr;
+	bool	m_swapped;
+	
+public:
+	btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped);
+
+	btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
+		: btActivatingCollisionAlgorithm(ci) {}
+
+	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+		{
+			
+			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
+
+			return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped);
+		}
+	};
+
+};
+
+#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -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.
+*/
+
+#include "btUnionFind.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 ) const
+		{
+			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);
+#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+		m_elements[i].m_sz = i;
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+	}
+	
+	 // Sort the vector using predicate and std::sort
+	  //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+	  m_elements.quickSort(btUnionFindElementSortPredicate());
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionDispatch/btUnionFind.h

@@ -0,0 +1,129 @@
+/*
+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_UNION_FIND_H
+#define BT_UNION_FIND_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#define USE_PATH_COMPRESSION 1
+
+///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
+#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 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)
+		{ 
+			//btAssert(x < m_N);
+			//btAssert(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
+				const btElement* elementPtr = &m_elements[m_elements[x].m_id];
+				m_elements[x].m_id = elementPtr->m_id;
+				x = elementPtr->m_id;			
+		#else//
+				x = m_elements[x].m_id;
+		#endif		
+				//btAssert(x < m_N);
+				//btAssert(x >= 0);
+
+			}
+			return x; 
+		}
+
+
+  };
+
+
+#endif //BT_UNION_FIND_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBox2dShape.cpp

@@ -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 "btBox2dShape.h"
+
+
+//{ 
+
+
+void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+{
+	btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
+}
+
+
+void	btBox2dShape::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));
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBox2dShape.h

@@ -0,0 +1,371 @@
+/*
+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_OBB_BOX_2D_SHAPE_H
+#define BT_OBB_BOX_2D_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMinMax.h"
+
+///The btBox2dShape 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.
+ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape
+{
+
+	//btVector3	m_boxHalfExtents1; //use m_implicitShapeDimensions instead
+
+	btVector3 m_centroid;
+	btVector3 m_vertices[4];
+	btVector3 m_normals[4];
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	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())); 
+		}
+
+	}
+
+
+	///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
+	btBox2dShape( const btVector3& boxHalfExtents) 
+		: btPolyhedralConvexShape(),
+		m_centroid(0,0,0)
+	{
+		m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
+		m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0);
+		m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0);
+		m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0);
+
+		m_normals[0].setValue(0,-1,0);
+		m_normals[1].setValue(1,0,0);
+		m_normals[2].setValue(0,1,0);
+		m_normals[3].setValue(-1,0,0);
+
+		btScalar minDimension = boxHalfExtents.getX();
+		if (minDimension>boxHalfExtents.getY())
+			minDimension = boxHalfExtents.getY();
+		setSafeMargin(minDimension);
+
+		m_shapeType = BOX_2D_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;
+
+
+
+
+
+	int	getVertexCount() const
+	{
+		return 4;
+	}
+
+	virtual int getNumVertices()const
+	{
+		return 4;
+	}
+
+	const btVector3* getVertices() const
+	{
+		return &m_vertices[0];
+	}
+
+	const btVector3* getNormals() const
+	{
+		return &m_normals[0];
+	}
+
+
+
+
+
+
+
+	virtual void getPlane(btVector3& planeNormal,btVector3& 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);
+	}
+
+
+	const btVector3& getCentroid() const
+	{
+		return m_centroid;
+	}
+	
+	virtual int getNumPlanes() const
+	{
+		return 6;
+	}	
+	
+	
+
+	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.),-halfExtents.x());
+			break;
+		case 1:
+			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+			break;
+		case 2:
+			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
+			break;
+		case 3:
+			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
+			break;
+		case 4:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
+			break;
+		case 5:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
+			break;
+		default:
+			btAssert(0);
+		}
+	}
+
+	
+	virtual void getEdge(int i,btVector3& pa,btVector3& 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 btVector3& 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 "Box2d";
+	}
+
+	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:
+			btAssert(0);
+		}
+	}
+
+};
+
+#endif //BT_OBB_BOX_2D_SHAPE_H
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBoxShape.cpp

@@ -0,0 +1,51 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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"
+
+btBoxShape::btBoxShape( const btVector3& boxHalfExtents) 
+: btPolyhedralConvexShape()
+{
+	m_shapeType = BOX_SHAPE_PROXYTYPE;
+
+	setSafeMargin(boxHalfExtents);
+
+	btVector3 margin(getMargin(),getMargin(),getMargin());
+	m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin;
+};
+
+
+
+
+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));
+
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBoxShape.h

@@ -0,0 +1,314 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_OBB_BOX_MINKOWSKI_H
+#define BT_OBB_BOX_MINKOWSKI_H
+
+#include "btPolyhedralConvexShape.h"
+#include "btCollisionMargin.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "LinearMath/btVector3.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.
+ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape
+{
+
+	//btVector3	m_boxHalfExtents1; //use m_implicitShapeDimensions instead
+
+
+public:
+
+BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btVector3 getHalfExtentsWithMargin() const
+	{
+		btVector3 halfExtents = getHalfExtentsWithoutMargin();
+		btVector3 margin(getMargin(),getMargin(),getMargin());
+		halfExtents += margin;
+		return halfExtents;
+	}
+	
+	const btVector3& getHalfExtentsWithoutMargin() const
+	{
+		return m_implicitShapeDimensions;//scaling is included, margin is not
+	}
+	
+
+	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);
+
+	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,btVector3& 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 = getHalfExtentsWithMargin();
+
+		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.),-halfExtents.x());
+			break;
+		case 1:
+			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
+			break;
+		case 2:
+			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
+			break;
+		case 3:
+			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
+			break;
+		case 4:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
+			break;
+		case 5:
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
+			break;
+		default:
+			btAssert(0);
+		}
+	}
+
+	
+	virtual void getEdge(int i,btVector3& pa,btVector3& 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 btVector3& 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:
+			btAssert(0);
+		}
+	}
+
+};
+
+
+#endif //BT_OBB_BOX_MINKOWSKI_H
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -0,0 +1,467 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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"
+#include "LinearMath/btSerializer.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_triangleInfoMap(0),
+m_useQuantizedAabbCompression(useQuantizedAabbCompression),
+m_ownsBvh(false)
+{
+	m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	//construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+	if (buildBvh)
+	{
+		buildOptimizedBvh();
+	}
+
+#endif //DISABLE_BVH
+
+}
+
+btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh)
+:btTriangleMeshShape(meshInterface),
+m_bvh(0),
+m_triangleInfoMap(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];
+				
+				if (type == PHY_FLOAT)
+				{
+					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					
+					m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+				}
+				else
+				{
+					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+					
+					m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(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];
+
+				if (type == PHY_FLOAT)
+				{
+					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+
+					m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());		
+				}
+				else
+				{
+					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+					
+					m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(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];
+		int m_numOverlap;
+
+		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
+			:m_meshInterface(meshInterface),
+			m_callback(callback),
+			m_numOverlap(0)
+		{
+		}
+				
+		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
+		{
+			m_numOverlap++;
+			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||indicestype==PHY_UCHAR);
+	
+			const btVector3& meshScaling = m_meshInterface->getScaling();
+			for (int j=2;j>=0;j--)
+			{
+				
+				int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j];
+
+
+#ifdef DEBUG_TRIANGLE_MESH
+				printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+				if (type == PHY_FLOAT)
+				{
+					float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+					
+					m_triangle[j] = btVector3(
+																		graphicsbase[0]*meshScaling.getX(),
+																		graphicsbase[1]*meshScaling.getY(),
+																		graphicsbase[2]*meshScaling.getZ());
+				}
+				else
+				{
+					double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+
+					m_triangle[j] = btVector3(
+						btScalar(graphicsbase[0])*meshScaling.getX(),
+						btScalar(graphicsbase[1])*meshScaling.getY(),
+						btScalar(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);
+	  buildOptimizedBvh();
+   }
+}
+
+void   btBvhTriangleMeshShape::buildOptimizedBvh()
+{
+	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);
+   }
+}
+
+
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer;
+
+	btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
+
+	m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
+
+	trimeshData->m_collisionMargin = float(m_collisionMargin);
+
+	
+
+	if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH))
+	{
+		void* chunk = serializer->findPointer(m_bvh);
+		if (chunk)
+		{
+#ifdef BT_USE_DOUBLE_PRECISION
+			trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk;
+			trimeshData->m_quantizedFloatBvh = 0;
+#else
+			trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)chunk;
+			trimeshData->m_quantizedDoubleBvh= 0;
+#endif //BT_USE_DOUBLE_PRECISION
+		} else
+		{
+
+#ifdef BT_USE_DOUBLE_PRECISION
+			trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
+			trimeshData->m_quantizedFloatBvh = 0;
+#else
+			trimeshData->m_quantizedFloatBvh  = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
+			trimeshData->m_quantizedDoubleBvh= 0;
+#endif //BT_USE_DOUBLE_PRECISION
+	
+			int sz = m_bvh->calculateSerializeBufferSizeNew();
+			btChunk* chunk = serializer->allocate(sz,1);
+			const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh);
+		}
+	} else
+	{
+		trimeshData->m_quantizedFloatBvh = 0;
+		trimeshData->m_quantizedDoubleBvh = 0;
+	}
+
+	
+
+	if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP))
+	{
+		void* chunk = serializer->findPointer(m_triangleInfoMap);
+		if (chunk)
+		{
+			trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk;
+		} else
+		{
+			trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap);
+			int sz = m_triangleInfoMap->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(sz,1);
+			const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap);
+		}
+	} else
+	{
+		trimeshData->m_triangleInfoMap = 0;
+	}
+
+	return "btTriangleMeshShapeData";
+}
+
+void	btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
+{
+	if (m_bvh)
+	{
+		int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place
+		btChunk* chunk = serializer->allocate(len,1);
+		const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
+		serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh);
+	}
+}
+
+void	btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
+{
+	if (m_triangleInfoMap)
+	{
+		int len = m_triangleInfoMap->calculateSerializeBufferSize();
+		btChunk* chunk = serializer->allocate(len,1);
+		const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
+		serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap);
+	}
+}
+
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -0,0 +1,149 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_BVH_TRIANGLE_MESH_SHAPE_H
+#define BT_BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "btTriangleMeshShape.h"
+#include "btOptimizedBvh.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "btTriangleInfoMap.h"
+
+///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects.
+///If you required moving concave triangle meshes, it is recommended to perform convex decomposition
+///using HACD, see Bullet/Demos/ConvexDecompositionDemo. 
+///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes.
+///btBvhTriangleMeshShape has 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;
+	btTriangleInfoMap*	m_triangleInfoMap;
+
+	bool m_useQuantizedAabbCompression;
+	bool m_ownsBvh;
+#ifdef __clang__
+	bool m_pad[11] __attribute__((unused));////need padding due to alignment
+#else
+	bool m_pad[11];////need padding due to alignment
+#endif
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	
+	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));
+
+	void    buildOptimizedBvh();
+
+	bool	usesQuantizedAabbCompression() const
+	{
+		return	m_useQuantizedAabbCompression;
+	}
+
+	void	setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap)
+	{
+		m_triangleInfoMap = triangleInfoMap;
+	}
+
+	const btTriangleInfoMap*	getTriangleInfoMap() const
+	{
+		return m_triangleInfoMap;
+	}
+	
+	btTriangleInfoMap*	getTriangleInfoMap()
+	{
+		return m_triangleInfoMap;
+	}
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	virtual void	serializeSingleBvh(btSerializer* serializer) const;
+
+	virtual void	serializeSingleTriangleInfoMap(btSerializer* serializer) const;
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btTriangleMeshShapeData
+{
+	btCollisionShapeData	m_collisionShapeData;
+
+	btStridingMeshInterfaceData m_meshInterface;
+
+	btQuantizedBvhFloatData		*m_quantizedFloatBvh;
+	btQuantizedBvhDoubleData	*m_quantizedDoubleBvh;
+
+	btTriangleInfoMapData	*m_triangleInfoMap;
+	
+	float	m_collisionMargin;
+
+	char m_pad3[4];
+	
+};
+
+
+SIMD_FORCE_INLINE	int	btBvhTriangleMeshShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btTriangleMeshShapeData);
+}
+
+
+
+#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp

@@ -0,0 +1,171 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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(-BT_LARGE_FLOAT));
+
+	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*(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*(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(-BT_LARGE_FLOAT));
+		const btVector3& vec = vectors[j];
+
+		btVector3 vtx;
+		btScalar newDot;
+		{
+			btVector3 pos(0,0,0);
+			pos[getUpAxis()] = getHalfHeight();
+			vtx = pos +vec*(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*(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);
+}
+
+
+
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCapsuleShape.h

@@ -0,0 +1,194 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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.
+ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape
+{
+protected:
+	int	m_upAxis;
+
+protected:
+	///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
+	btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
+
+public:
+	
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	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 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 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();  
+			btVector3 center = t.getOrigin();
+            btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+        
+			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];
+	}
+
+	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 btVector3	getAnisotropicRollingFrictionDirection() const
+	{
+		btVector3 aniDir(0,0,0);
+		aniDir[getUpAxis()]=1;
+		return aniDir;
+	}
+
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	SIMD_FORCE_INLINE	void	deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
+
+};
+
+///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";
+	}
+
+	
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCapsuleShapeData
+{
+	btConvexInternalShapeData	m_convexInternalShapeData;
+
+	int	m_upAxis;
+
+	char	m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btCapsuleShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btCapsuleShapeData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer;
+	
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+
+	shapeData->m_upAxis = m_upAxis;
+	
+	return "btCapsuleShapeData";
+}
+
+SIMD_FORCE_INLINE	void	btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
+{
+	m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
+	m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
+	m_localScaling.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_localScaling);
+	//it is best to already pre-allocate the matching btCapsuleShape*(X/Z) version to match m_upAxis
+	m_upAxis = dataBuffer->m_upAxis;
+}
+
+#endif //BT_CAPSULE_SHAPE_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCollisionMargin.h

@@ -0,0 +1,27 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_MARGIN_H
+#define BT_COLLISION_MARGIN_H
+
+///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape.
+///This collision margin is used by Gjk and some other algorithms
+///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API
+#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01)
+
+
+
+#endif //BT_COLLISION_MARGIN_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCollisionShape.cpp

@@ -0,0 +1,119 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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"
+#include "LinearMath/btSerializer.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::getContactBreakingThreshold(btScalar defaultContactThreshold) const
+{
+	return getAngularMotionDisc() * defaultContactThreshold;
+}
+
+btScalar	btCollisionShape::getAngularMotionDisc() const
+{
+	///@todo cache this value, to improve performance
+	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;
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer;
+	char* name = (char*) serializer->findNameForPointer(this);
+	shapeData->m_name = (char*)serializer->getUniquePointer(name);
+	if (shapeData->m_name)
+	{
+		serializer->serializeName(name);
+	}
+	shapeData->m_shapeType = m_shapeType;
+	//shapeData->m_padding//??
+	return "btCollisionShapeData";
+}
+
+void	btCollisionShape::serializeSingleShape(btSerializer* serializer) const
+{
+	int len = calculateSerializeBufferSize();
+	btChunk* chunk = serializer->allocate(len,1);
+	const char* structType = serialize(chunk->m_oldPtr, serializer);
+	serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCollisionShape.h

@@ -0,0 +1,170 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_SHAPE_H
+#define BT_COLLISION_SHAPE_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types
+class btSerializer;
+
+
+///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects.
+ATTRIBUTE_ALIGNED16(class) btCollisionShape
+{
+protected:
+	int m_shapeType;
+	void* m_userPointer;
+	int m_userIndex;
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
+	{
+	}
+
+	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;
+
+	virtual btScalar	getContactBreakingThreshold(btScalar defaultContactThresholdFactor) 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;
+
+
+
+	SIMD_FORCE_INLINE bool	isPolyhedral() const
+	{
+		return btBroadphaseProxy::isPolyhedral(getShapeType());
+	}
+
+	SIMD_FORCE_INLINE bool	isConvex2d() const
+	{
+		return btBroadphaseProxy::isConvex2d(getShapeType());
+	}
+
+	SIMD_FORCE_INLINE bool	isConvex() const
+	{
+		return btBroadphaseProxy::isConvex(getShapeType());
+	}
+	SIMD_FORCE_INLINE bool	isNonMoving() const
+	{
+		return btBroadphaseProxy::isNonMoving(getShapeType());
+	}
+	SIMD_FORCE_INLINE bool	isConcave() const
+	{
+		return btBroadphaseProxy::isConcave(getShapeType());
+	}
+	SIMD_FORCE_INLINE bool	isCompound() const
+	{
+		return btBroadphaseProxy::isCompound(getShapeType());
+	}
+
+	SIMD_FORCE_INLINE bool	isSoftBody() const
+	{
+		return btBroadphaseProxy::isSoftBody(getShapeType());
+	}
+
+	///isInfinite is used to catch simulation error (aabb check)
+	SIMD_FORCE_INLINE bool isInfinite() const
+	{
+		return btBroadphaseProxy::isInfinite(getShapeType());
+	}
+
+#ifndef __SPU__
+	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; }
+
+	///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction
+	///See Bullet/Demos/RollingFrictionDemo for an example
+	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	{
+		return btVector3(1,1,1);
+	}
+	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;
+	}
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	virtual void	serializeSingleShape(btSerializer* serializer) const;
+
+};	
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCollisionShapeData
+{
+	char	*m_name;
+	int		m_shapeType;
+	char	m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btCollisionShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btCollisionShapeData);
+}
+
+
+
+#endif //BT_COLLISION_SHAPE_H
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCompoundShape.cpp

@@ -0,0 +1,359 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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"
+#include "LinearMath/btSerializer.h"
+
+btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
+: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
+m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
+m_dynamicAabbTree(0),
+m_updateRevision(1),
+m_collisionMargin(btScalar(0.)),
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+{
+	m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
+
+	if (enableDynamicAabbTree)
+	{
+		void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+		m_dynamicAabbTree = new(mem) btDbvt();
+		btAssert(mem==m_dynamicAabbTree);
+	}
+
+	m_children.reserve(initialChildCapacity);
+}
+
+
+btCompoundShape::~btCompoundShape()
+{
+	if (m_dynamicAabbTree)
+	{
+		m_dynamicAabbTree->~btDbvt();
+		btAlignedFree(m_dynamicAabbTree);
+	}
+}
+
+void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
+{
+	m_updateRevision++;
+	//m_childTransforms.push_back(localTransform);
+	//m_childShapes.push_back(shape);
+	btCompoundShapeChild child;
+	child.m_node = 0;
+	child.m_transform = localTransform;
+	child.m_childShape = shape;
+	child.m_childShapeType = shape->getShapeType();
+	child.m_childMargin = shape->getMargin();
+
+	
+	//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);
+		size_t index = m_children.size();
+		child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
+	}
+
+	m_children.push_back(child);
+
+}
+
+void	btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
+{
+	m_children[childIndex].m_transform = newChildTransform;
+
+	if (m_dynamicAabbTree)
+	{
+		///update the dynamic aabb tree
+		btVector3 localAabbMin,localAabbMax;
+		m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
+		ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		//int index = m_children.size()-1;
+		m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
+	}
+
+	if (shouldRecalculateLocalAabb)
+	{
+		recalculateLocalAabb();
+	}
+}
+
+void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
+{
+	m_updateRevision++;
+	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);
+    if (m_dynamicAabbTree) 
+		m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
+	m_children.pop_back();
+
+}
+
+
+
+void btCompoundShape::removeChildShape(btCollisionShape* shape)
+{
+	m_updateRevision++;
+	// 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)
+		{
+			removeChildShapeByIndex(i);
+		}
+	}
+
+
+
+	recalculateLocalAabb();
+}
+
+void btCompoundShape::recalculateLocalAabb()
+{
+	// Recalculate the local aabb
+	// Brute force, it iterates over all the shapes left.
+
+	m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+	m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+	//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);
+	btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+	
+	//avoid an illegal AABB when there are no children
+	if (!m_children.size())
+	{
+		localHalfExtents.setValue(0,0,0);
+		localCenter.setValue(0,0,0);
+	}
+	localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
+		
+
+	btMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+	btVector3 center = trans(localCenter);
+
+    btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	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);
+	int k;
+
+	for (k = 0; k < n; k++)
+	{
+		btAssert(masses[k]>0);
+		center += m_children[k].m_transform.getOrigin() * masses[k];
+		totalMass += masses[k];
+	}
+
+	btAssert(totalMass>0);
+
+	center /= totalMass;
+	principal.setOrigin(center);
+
+	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	for ( 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]);
+}
+
+
+
+
+
+void btCompoundShape::setLocalScaling(const btVector3& scaling)
+{
+
+	for(int i = 0; i < m_children.size(); i++)
+	{
+		btTransform childTrans = getChildTransform(i);
+		btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
+//		childScale = childScale * (childTrans.getBasis() * scaling);
+		childScale = childScale * scaling / m_localScaling;
+		m_children[i].m_childShape->setLocalScaling(childScale);
+		childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
+		updateChildTransform(i, childTrans,false);
+	}
+	
+	m_localScaling = scaling;
+	recalculateLocalAabb();
+
+}
+
+
+void btCompoundShape::createAabbTreeFromChildren()
+{
+    if ( !m_dynamicAabbTree )
+    {
+        void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+        m_dynamicAabbTree = new(mem) btDbvt();
+        btAssert(mem==m_dynamicAabbTree);
+
+        for ( int index = 0; index < m_children.size(); index++ )
+        {
+            btCompoundShapeChild &child = m_children[index];
+
+            //extend the local aabbMin/aabbMax
+            btVector3 localAabbMin,localAabbMax;
+            child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
+
+            const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+			size_t index2 = index;
+            child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
+        }
+    }
+}
+
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
+	btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
+
+	shapeData->m_collisionMargin = float(m_collisionMargin);
+	shapeData->m_numChildShapes = m_children.size();
+	shapeData->m_childShapePtr = 0;
+	if (shapeData->m_numChildShapes)
+	{
+		btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
+		btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
+		shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
+
+		for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
+		{
+			memPtr->m_childMargin = float(m_children[i].m_childMargin);
+			memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
+			//don't serialize shapes that already have been serialized
+			if (!serializer->findPointer(m_children[i].m_childShape))
+			{
+				btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
+				const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
+				serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
+			} 
+
+			memPtr->m_childShapeType = m_children[i].m_childShapeType;
+			m_children[i].m_transform.serializeFloat(memPtr->m_transform);
+		}
+		serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
+	}
+	return "btCompoundShapeData";
+}
+

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -0,0 +1,212 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_COMPOUND_SHAPE_H
+#define BT_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 );
+}
+
+/// The btCompoundShape allows to store multiple other btCollisionShapes
+/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape.
+/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. 
+/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25
+/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
+ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
+{
+protected:
+	btAlignedObjectArray<btCompoundShapeChild> m_children;
+	btVector3						m_localAabbMin;
+	btVector3						m_localAabbMax;
+
+	btDbvt*							m_dynamicAabbTree;
+
+	///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
+	int								m_updateRevision;
+
+	btScalar	m_collisionMargin;
+
+	btVector3	m_localScaling;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	explicit btCompoundShape(bool enableDynamicAabbTree = true, const int initialChildCapacity = 0);
+
+	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;
+	}
+
+	///set a new transform for a child, and update internal data structures (local aabb and dynamic tree)
+	void	updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true);
+
+
+	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);
+
+	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";
+	}
+
+	const btDbvt*	getDynamicAabbTree() const
+	{
+		return m_dynamicAabbTree;
+	}
+	
+	btDbvt*	getDynamicAabbTree()
+	{
+		return m_dynamicAabbTree;
+	}
+
+	void createAabbTreeFromChildren();
+
+	///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;
+
+	int	getUpdateRevision() const
+	{
+		return m_updateRevision;
+	}
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btCompoundShapeChildData
+{
+	btTransformFloatData	m_transform;
+	btCollisionShapeData	*m_childShape;
+	int						m_childShapeType;
+	float					m_childMargin;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btCompoundShapeData
+{
+	btCollisionShapeData		m_collisionShapeData;
+
+	btCompoundShapeChildData	*m_childShapePtr;
+
+	int							m_numChildShapes;
+
+	float	m_collisionMargin;
+
+};
+
+
+SIMD_FORCE_INLINE	int	btCompoundShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btCompoundShapeData);
+}
+
+
+
+
+
+
+
+#endif //BT_COMPOUND_SHAPE_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btConcaveShape.cpp

@@ -0,0 +1,27 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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()
+{
+
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btConcaveShape.h

@@ -0,0 +1,62 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_CONCAVE_SHAPE_H
+#define BT_CONCAVE_SHAPE_H
+
+#include "btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
+#include "btTriangleCallback.h"
+
+/// PHY_ScalarType enumerates possible scalar types.
+/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use
+typedef enum PHY_ScalarType {
+	PHY_FLOAT,
+	PHY_DOUBLE,
+	PHY_INTEGER,
+	PHY_SHORT,
+	PHY_FIXEDPOINT88,
+	PHY_UCHAR
+} PHY_ScalarType;
+
+///The btConcaveShape class provides an interface for non-moving (static) concave shapes.
+///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape.
+ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape
+{
+protected:
+	btScalar m_collisionMargin;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	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 //BT_CONCAVE_SHAPE_H

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btConeShape.cpp

@@ -0,0 +1,147 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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"
+
+
+
+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:
+		btAssert(0);
+	};
+	
+	m_implicitShapeDimensions[m_coneIndices[0]] = m_radius;
+	m_implicitShapeDimensions[m_coneIndices[1]] = m_height;
+	m_implicitShapeDimensions[m_coneIndices[2]] = m_radius;
+}
+
+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;
+}
+
+
+void	btConeShape::setLocalScaling(const btVector3& scaling)
+{
+	int axis = m_coneIndices[1];
+	int r1 = m_coneIndices[0];
+	int r2 = m_coneIndices[2];
+	m_height *= scaling[axis] / m_localScaling[axis];
+	m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2;
+	m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
+	btConvexInternalShape::setLocalScaling(scaling);
+}

modules/bullet/bullet3-2.85.1/src/BulletCollision/CollisionShapes/btConeShape.h

@@ -0,0 +1,180 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_CONE_MINKOWSKI_H
+#define BT_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.
+ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape
+
+{
+
+	btScalar m_sinAngle;
+	btScalar m_radius;
+	btScalar m_height;
+	int		m_coneIndices[3];
+	btVector3 coneLocalSupport(const btVector3& v) const;
+
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	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;}
+
+	void setRadius(const btScalar radius)
+	{
+		m_radius = radius;
+	}
+	void setHeight(const btScalar height)
+	{
+		m_height = 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];
+		}
+
+	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	{
+		return btVector3 (0,1,0);
+	}
+
+	virtual void	setLocalScaling(const btVector3& scaling);
+	
+	
+	virtual	int	calculateSerializeBufferSize() const;
+	
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	
+
+};
+
+///btConeShape implements a Cone shape, around the X axis
+class btConeShapeX : public btConeShape
+{
+	public:
+		btConeShapeX(btScalar radius,btScalar height);
+
+	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	{
+		return btVector3 (1,0,0);
+	}
+
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "ConeX";
+	}
+	
+	
+};
+
+///btConeShapeZ implements a Cone shape, around the Z axis
+class btConeShapeZ : public btConeShape
+{
+public:
+	btConeShapeZ(btScalar radius,btScalar height);
+
+	virtual btVector3	getAnisotropicRollingFrictionDirection() const
+	{
+		return btVector3 (0,0,1);
+	}
+
+	//debugging
+	virtual const char*	getName()const
+	{
+		return "ConeZ";
+	}
+	
+	
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btConeShapeData
+{
+	btConvexInternalShapeData	m_convexInternalShapeData;
+	
+	int	m_upIndex;
+	
+	char	m_padding[4];
+};
+
+SIMD_FORCE_INLINE	int	btConeShape::calculateSerializeBufferSize() const
+{
+	return sizeof(btConeShapeData);
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
+	
+	btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+	
+	shapeData->m_upIndex = m_coneIndices[1];
+	
+	return "btConeShapeData";
+}
+
+#endif //BT_CONE_MINKOWSKI_H
+