Merge branch 'Preview4_0' into alpha40translucency2

Engine/lib/bullet/.travis.yml

@@ -1,27 +1,56 @@
 language: cpp
-os:
-  - linux
-  - osx
-compiler:
-  - gcc
-  - clang
-addons:
-  apt:
-    packages:
-      - python3
-
+matrix:
+  include:
+    - os: linux
+      compiler: gcc
+      env:
+        - BUILD_NAME=TRUSTY_GCC
+        - SUDO=sudo
+    - os: linux
+      compiler: clang
+      env:
+        - BUILD_NAME=TRUSTY_CLANG
+        - SUDO=sudo
+    - os: linux
+      compiler: gcc
+      env:
+        - BUILD_NAME=XENIAL_GCC
+        - DOCKER_FILE="ubuntu-xenial"
+      services: docker
+    - os: linux
+      compiler: clang
+      env:
+        - BUILD_NAME=XENIAL_CLANG
+        - DOCKER_FILE="ubuntu-xenial"
+      services: docker
+    - os: linux
+      compiler: gcc
+      env:
+        - BUILD_NAME=BIONIC_GCC
+        - DOCKER_FILE="ubuntu-bionic"
+      services: docker
+    - os: linux
+      compiler: clang
+      env:
+        - BUILD_NAME=BIONIC_CLANG
+        - DOCKER_FILE="ubuntu-bionic"
+      services: docker
+    - os: osx
+      compiler: gcc
+      env:
+        - BUILD_NAME=OSX_GCC
+    - os: osx
+      compiler: clang
+      env:
+        - BUILD_NAME=OSX_CLANG
+before_install:
+  - if [ -n "$DOCKER_FILE" ]; then
+      docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .;
+      docker run -itd -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR --env-file .ci/docker/env.list --name bullet-docker "$DOCKER_FILE";
+    fi
 script:
-  - echo "CXX="$CXX
-  - echo "CC="$CC
-  - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
-  - make -j8
-  - ctest -j8 --output-on-failure
-  # Build again with double precision
-  - cmake . -G "Unix Makefiles" -DUSE_DOUBLE_PRECISION=ON #-DCMAKE_CXX_FLAGS=-Werror
-  - make -j8
-  - ctest -j8 --output-on-failure
-  # Build again with shared libraries
-  - cmake . -G "Unix Makefiles" -DBUILD_SHARED_LIBS=ON
-  - make -j8
-  - ctest -j8 --output-on-failure
-  - sudo make install
+  - if [ -n "$DOCKER_FILE" ]; then
+      docker exec bullet-docker /bin/sh -c "cd $TRAVIS_BUILD_DIR && ./.ci/script.sh";
+    else
+      '.ci/script.sh';
+    fi

Engine/lib/bullet/CMakeLists.txt

@@ -26,16 +26,16 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
 OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
-OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
+OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON)
 
-OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
-IF (BULLET2_USE_THREAD_LOCKS)
+OPTION(BULLET2_MULTITHREADING "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
+IF (BULLET2_MULTITHREADING)
     OPTION(BULLET2_USE_OPEN_MP_MULTITHREADING "Build Bullet 2 with support for multi-threading with OpenMP (requires a compiler with OpenMP support)" OFF)
     OPTION(BULLET2_USE_TBB_MULTITHREADING "Build Bullet 2 with support for multi-threading with Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
     IF (MSVC)
         OPTION(BULLET2_USE_PPL_MULTITHREADING "Build Bullet 2 with support for multi-threading with Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
     ENDIF (MSVC)
-ENDIF (BULLET2_USE_THREAD_LOCKS)
+ENDIF (BULLET2_MULTITHREADING)
 
 
 IF(NOT WIN32)
@@ -108,7 +108,7 @@ IF(MSVC)
 	IF (CMAKE_CL_64)
 	  ADD_DEFINITIONS(-D_WIN64)
 	ELSE()
-	  OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option"	ON)
+	  OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option"	OFF)
 	  option(USE_MSVC_SSE2 "Compile your program with SSE2 instructions" ON)
 
 	  IF (USE_MSVC_SSE)
@@ -217,20 +217,23 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
 SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION")
 ENDIF (USE_DOUBLE_PRECISION)
 
-IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
-ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
-ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+IF (NOT USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ADD_DEFINITIONS(-DSKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ENDIF ()
 
 IF(USE_GRAPHICAL_BENCHMARK)
 ADD_DEFINITIONS( -DUSE_GRAPHICAL_BENCHMARK)
 ENDIF (USE_GRAPHICAL_BENCHMARK)
 
-IF(BULLET2_USE_THREAD_LOCKS)
+IF(BULLET2_MULTITHREADING)
 	ADD_DEFINITIONS( -DBT_THREADSAFE=1 )
 	IF (NOT MSVC)
 		SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 	ENDIF (NOT MSVC)
-ENDIF (BULLET2_USE_THREAD_LOCKS)
+	IF (NOT WIN32)
+		LINK_LIBRARIES( pthread )
+	ENDIF (NOT WIN32)
+ENDIF (BULLET2_MULTITHREADING)
 
 IF (BULLET2_USE_OPEN_MP_MULTITHREADING)
     ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
@@ -301,25 +304,26 @@ IF (APPLE)
 ENDIF()
 
 OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
+
 # Optional Python configuration
-# builds pybullet automatically if all the requirements are met
-SET(PYTHON_VERSION_PYBULLET "" CACHE STRING "Python version pybullet will use.")
-SET(Python_ADDITIONAL_VERSIONS 3 3.6 3.5 3.4 3.3 3.2 3.1 3.0 2.7 2.7.12 2.7.10 2.7.3 )
-SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS})
-SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH})
-OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF)
-IF(EXACT_PYTHON_VERSION)
-  set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED)
-ENDIF(EXACT_PYTHON_VERSION)
-# first find the python interpreter
-FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG})
-# python library should exactly match that of the interpreter
-FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT)
-SET(DEFAULT_BUILD_PYBULLET OFF)
-IF(PYTHONLIBS_FOUND)
-	SET(DEFAULT_BUILD_PYBULLET ON)
-ENDIF(PYTHONLIBS_FOUND)
-OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" ${DEFAULT_BUILD_PYBULLET})
+# Will not probe environment for Python configuration (which can abort the
+# build process) unless you explicitly turn on BUILD_PYBULLET.
+OPTION(BUILD_PYBULLET "Set when you want to build pybullet (Python bindings for Bullet)" OFF)
+IF(BUILD_PYBULLET)
+	SET(PYTHON_VERSION_PYBULLET "" CACHE STRING "Python version pybullet will use.")
+	SET(Python_ADDITIONAL_VERSIONS 3 3.6 3.5 3.4 3.3 3.2 3.1 3.0 2.7 2.7.12 2.7.10 2.7.3 )
+	SET_PROPERTY(CACHE PYTHON_VERSION_PYBULLET PROPERTY STRINGS ${Python_ADDITIONAL_VERSIONS})
+	SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/build3/cmake ${CMAKE_MODULE_PATH})
+	OPTION(EXACT_PYTHON_VERSION "Require Python and match PYTHON_VERSION_PYBULLET exactly, e.g. 2.7.12" OFF)
+	IF(EXACT_PYTHON_VERSION)
+	    set(EXACT_PYTHON_VERSION_FLAG EXACT REQUIRED)
+	ENDIF(EXACT_PYTHON_VERSION)
+	# first find the python interpreter
+	FIND_PACKAGE(PythonInterp ${PYTHON_VERSION_PYBULLET} ${EXACT_PYTHON_VERSION_FLAG})
+	# python library should exactly match that of the interpreter
+	# the following can result in fatal error if you don't have the right python configuration
+	FIND_PACKAGE(PythonLibs ${PYTHON_VERSION_STRING} EXACT)
+ENDIF(BUILD_PYBULLET)
 
 OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON)
 OPTION(BUILD_CLSOCKET "Set when you want to build apps with enet TCP networking support" ON)
@@ -365,6 +369,13 @@ IF(BUILD_PYBULLET)
 
 ENDIF(BUILD_PYBULLET)
 
+IF(NOT WIN32 AND NOT APPLE)
+    OPTION(BUILD_EGL "Build OpenGL/EGL" ON)
+    IF(BUILD_EGL)
+        ADD_DEFINITIONS(-DBT_USE_EGL)
+    ENDIF(BUILD_EGL)
+ENDIF()
+
 IF(BUILD_BULLET3)
 	 IF(APPLE)
 		MESSAGE("Mac OSX Version is ${_CURRENT_OSX_VERSION}")

Engine/lib/bullet/README.md

@@ -1,4 +1,3 @@
-
 [![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)
 
@@ -6,7 +5,7 @@
 
 This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
 
-New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR
+New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
 
 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:
@@ -80,9 +79,9 @@ Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the followin
 Using premake:
 ```
 	cd build3
-	./premake4_linux gmake --double
-	./premake4_linux64 gmake --double
-	./premake4_osx gmake --double --enable_pybullet
+	./premake4_linux --double gmake
+	./premake4_linux64 --double gmake
+	./premake4_osx --double --enable_pybullet gmake
 ```
 Then
 ```

Engine/lib/bullet/VERSION

@@ -1 +1 @@
-2.87
+2.88

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h

@@ -0,0 +1,38 @@
+/*
+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 B3_BROADPHASE_CALLBACK_H
+#define B3_BROADPHASE_CALLBACK_H
+
+#include "Bullet3Common/b3Vector3.h"
+struct b3BroadphaseProxy;
+
+struct b3BroadphaseAabbCallback
+{
+	virtual ~b3BroadphaseAabbCallback() {}
+	virtual bool process(const b3BroadphaseProxy* proxy) = 0;
+};
+
+struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
+{
+	///added some cached data to accelerate ray-AABB tests
+	b3Vector3 m_rayDirectionInverse;
+	unsigned int m_signs[3];
+	b3Scalar m_lambda_max;
+
+	virtual ~b3BroadphaseRayCallback() {}
+};
+
+#endif  //B3_BROADPHASE_CALLBACK_H

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp

@@ -0,0 +1,1352 @@
+/*
+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.
+*/
+///b3DynamicBvh implementation by Nathanael Presson
+
+#include "b3DynamicBvh.h"
+
+//
+typedef b3AlignedObjectArray<b3DbvtNode*> b3NodeArray;
+typedef b3AlignedObjectArray<const b3DbvtNode*> b3ConstNodeArray;
+
+//
+struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide
+{
+	b3ConstNodeArray nodes;
+	void Process(const b3DbvtNode* n) { nodes.push_back(n); }
+};
+
+//
+static B3_DBVT_INLINE int b3IndexOf(const b3DbvtNode* node)
+{
+	return (node->parent->childs[1] == node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtVolume b3Merge(const b3DbvtVolume& a,
+										   const b3DbvtVolume& b)
+{
+#if (B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE)
+	B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]);
+	b3DbvtVolume& res = *(b3DbvtVolume*)locals;
+#else
+	b3DbvtVolume res;
+#endif
+	b3Merge(a, b, res);
+	return (res);
+}
+
+// volume+edge lengths
+static B3_DBVT_INLINE b3Scalar b3Size(const b3DbvtVolume& a)
+{
+	const b3Vector3 edges = a.Lengths();
+	return (edges.x * edges.y * edges.z +
+			edges.x + edges.y + edges.z);
+}
+
+//
+static void b3GetMaxDepth(const b3DbvtNode* node, int depth, int& maxdepth)
+{
+	if (node->isinternal())
+	{
+		b3GetMaxDepth(node->childs[0], depth + 1, maxdepth);
+		b3GetMaxDepth(node->childs[1], depth + 1, maxdepth);
+	}
+	else
+		maxdepth = b3Max(maxdepth, depth);
+}
+
+//
+static B3_DBVT_INLINE void b3DeleteNode(b3DynamicBvh* pdbvt,
+										b3DbvtNode* node)
+{
+	b3AlignedFree(pdbvt->m_free);
+	pdbvt->m_free = node;
+}
+
+//
+static void b3RecurseDeleteNode(b3DynamicBvh* pdbvt,
+								b3DbvtNode* node)
+{
+	if (!node->isleaf())
+	{
+		b3RecurseDeleteNode(pdbvt, node->childs[0]);
+		b3RecurseDeleteNode(pdbvt, node->childs[1]);
+	}
+	if (node == pdbvt->m_root) pdbvt->m_root = 0;
+	b3DeleteNode(pdbvt, node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
+											   b3DbvtNode* parent,
+											   void* data)
+{
+	b3DbvtNode* node;
+	if (pdbvt->m_free)
+	{
+		node = pdbvt->m_free;
+		pdbvt->m_free = 0;
+	}
+	else
+	{
+		node = new (b3AlignedAlloc(sizeof(b3DbvtNode), 16)) b3DbvtNode();
+	}
+	node->parent = parent;
+	node->data = data;
+	node->childs[1] = 0;
+	return (node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
+											   b3DbvtNode* parent,
+											   const b3DbvtVolume& volume,
+											   void* data)
+{
+	b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
+	node->volume = volume;
+	return (node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode* b3CreateNode(b3DynamicBvh* pdbvt,
+											   b3DbvtNode* parent,
+											   const b3DbvtVolume& volume0,
+											   const b3DbvtVolume& volume1,
+											   void* data)
+{
+	b3DbvtNode* node = b3CreateNode(pdbvt, parent, data);
+	b3Merge(volume0, volume1, node->volume);
+	return (node);
+}
+
+//
+static void b3InsertLeaf(b3DynamicBvh* pdbvt,
+						 b3DbvtNode* root,
+						 b3DbvtNode* leaf)
+{
+	if (!pdbvt->m_root)
+	{
+		pdbvt->m_root = leaf;
+		leaf->parent = 0;
+	}
+	else
+	{
+		if (!root->isleaf())
+		{
+			do
+			{
+				root = root->childs[b3Select(leaf->volume,
+											 root->childs[0]->volume,
+											 root->childs[1]->volume)];
+			} while (!root->isleaf());
+		}
+		b3DbvtNode* prev = root->parent;
+		b3DbvtNode* node = b3CreateNode(pdbvt, prev, leaf->volume, root->volume, 0);
+		if (prev)
+		{
+			prev->childs[b3IndexOf(root)] = node;
+			node->childs[0] = root;
+			root->parent = node;
+			node->childs[1] = leaf;
+			leaf->parent = node;
+			do
+			{
+				if (!prev->volume.Contain(node->volume))
+					b3Merge(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 b3DbvtNode* b3RemoveLeaf(b3DynamicBvh* pdbvt,
+								b3DbvtNode* leaf)
+{
+	if (leaf == pdbvt->m_root)
+	{
+		pdbvt->m_root = 0;
+		return (0);
+	}
+	else
+	{
+		b3DbvtNode* parent = leaf->parent;
+		b3DbvtNode* prev = parent->parent;
+		b3DbvtNode* sibling = parent->childs[1 - b3IndexOf(leaf)];
+		if (prev)
+		{
+			prev->childs[b3IndexOf(parent)] = sibling;
+			sibling->parent = prev;
+			b3DeleteNode(pdbvt, parent);
+			while (prev)
+			{
+				const b3DbvtVolume pb = prev->volume;
+				b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
+				if (b3NotEqual(pb, prev->volume))
+				{
+					prev = prev->parent;
+				}
+				else
+					break;
+			}
+			return (prev ? prev : pdbvt->m_root);
+		}
+		else
+		{
+			pdbvt->m_root = sibling;
+			sibling->parent = 0;
+			b3DeleteNode(pdbvt, parent);
+			return (pdbvt->m_root);
+		}
+	}
+}
+
+//
+static void b3FetchLeaves(b3DynamicBvh* pdbvt,
+						  b3DbvtNode* root,
+						  b3NodeArray& leaves,
+						  int depth = -1)
+{
+	if (root->isinternal() && depth)
+	{
+		b3FetchLeaves(pdbvt, root->childs[0], leaves, depth - 1);
+		b3FetchLeaves(pdbvt, root->childs[1], leaves, depth - 1);
+		b3DeleteNode(pdbvt, root);
+	}
+	else
+	{
+		leaves.push_back(root);
+	}
+}
+
+static bool b3LeftOfAxis(const b3DbvtNode* node,
+						 const b3Vector3& org,
+						 const b3Vector3& axis)
+{
+	return b3Dot(axis, node->volume.Center() - org) <= 0;
+}
+
+// Partitions leaves such that leaves[0, n) are on the
+// left of axis, and leaves[n, count) are on the right
+// of axis. returns N.
+static int b3Split(b3DbvtNode** leaves,
+				   int count,
+				   const b3Vector3& org,
+				   const b3Vector3& axis)
+{
+	int begin = 0;
+	int end = count;
+	for (;;)
+	{
+		while (begin != end && b3LeftOfAxis(leaves[begin], org, axis))
+		{
+			++begin;
+		}
+
+		if (begin == end)
+		{
+			break;
+		}
+
+		while (begin != end && !b3LeftOfAxis(leaves[end - 1], org, axis))
+		{
+			--end;
+		}
+
+		if (begin == end)
+		{
+			break;
+		}
+
+		// swap out of place nodes
+		--end;
+		b3DbvtNode* temp = leaves[begin];
+		leaves[begin] = leaves[end];
+		leaves[end] = temp;
+		++begin;
+	}
+
+	return begin;
+}
+
+//
+static b3DbvtVolume b3Bounds(b3DbvtNode** leaves,
+							 int count)
+{
+#if B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE
+	B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]);
+	b3DbvtVolume& volume = *(b3DbvtVolume*)locals;
+	volume = leaves[0]->volume;
+#else
+	b3DbvtVolume volume = leaves[0]->volume;
+#endif
+	for (int i = 1, ni = count; i < ni; ++i)
+	{
+		b3Merge(volume, leaves[i]->volume, volume);
+	}
+	return (volume);
+}
+
+//
+static void b3BottomUp(b3DynamicBvh* pdbvt,
+					   b3DbvtNode** leaves,
+					   int count)
+{
+	while (count > 1)
+	{
+		b3Scalar minsize = B3_INFINITY;
+		int minidx[2] = {-1, -1};
+		for (int i = 0; i < count; ++i)
+		{
+			for (int j = i + 1; j < count; ++j)
+			{
+				const b3Scalar sz = b3Size(b3Merge(leaves[i]->volume, leaves[j]->volume));
+				if (sz < minsize)
+				{
+					minsize = sz;
+					minidx[0] = i;
+					minidx[1] = j;
+				}
+			}
+		}
+		b3DbvtNode* n[] = {leaves[minidx[0]], leaves[minidx[1]]};
+		b3DbvtNode* p = b3CreateNode(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[minidx[1]] = leaves[count - 1];
+		--count;
+	}
+}
+
+//
+static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt,
+							 b3DbvtNode** leaves,
+							 int count,
+							 int bu_treshold)
+{
+	static const b3Vector3 axis[] = {b3MakeVector3(1, 0, 0),
+									 b3MakeVector3(0, 1, 0),
+									 b3MakeVector3(0, 0, 1)};
+	b3Assert(bu_treshold > 1);
+	if (count > 1)
+	{
+		if (count > bu_treshold)
+		{
+			const b3DbvtVolume vol = b3Bounds(leaves, count);
+			const b3Vector3 org = vol.Center();
+			int partition;
+			int bestaxis = -1;
+			int bestmidp = count;
+			int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
+			int i;
+			for (i = 0; i < count; ++i)
+			{
+				const b3Vector3 x = leaves[i]->volume.Center() - org;
+				for (int j = 0; j < 3; ++j)
+				{
+					++splitcount[j][b3Dot(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)b3Fabs(b3Scalar(splitcount[i][0] - splitcount[i][1]));
+					if (midp < bestmidp)
+					{
+						bestaxis = i;
+						bestmidp = midp;
+					}
+				}
+			}
+			if (bestaxis >= 0)
+			{
+				partition = b3Split(leaves, count, org, axis[bestaxis]);
+				b3Assert(partition != 0 && partition != count);
+			}
+			else
+			{
+				partition = count / 2 + 1;
+			}
+			b3DbvtNode* node = b3CreateNode(pdbvt, 0, vol, 0);
+			node->childs[0] = b3TopDown(pdbvt, &leaves[0], partition, bu_treshold);
+			node->childs[1] = b3TopDown(pdbvt, &leaves[partition], count - partition, bu_treshold);
+			node->childs[0]->parent = node;
+			node->childs[1]->parent = node;
+			return (node);
+		}
+		else
+		{
+			b3BottomUp(pdbvt, leaves, count);
+			return (leaves[0]);
+		}
+	}
+	return (leaves[0]);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n, b3DbvtNode*& r)
+{
+	b3DbvtNode* p = n->parent;
+	b3Assert(n->isinternal());
+	if (p > n)
+	{
+		const int i = b3IndexOf(n);
+		const int j = 1 - i;
+		b3DbvtNode* s = p->childs[j];
+		b3DbvtNode* q = p->parent;
+		b3Assert(n == p->childs[i]);
+		if (q)
+			q->childs[b3IndexOf(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;
+		b3Swap(p->volume, n->volume);
+		return (p);
+	}
+	return (n);
+}
+
+#if 0
+static B3_DBVT_INLINE b3DbvtNode*	walkup(b3DbvtNode* n,int count)
+{
+	while(n&&(count--)) n=n->parent;
+	return(n);
+}
+#endif
+
+//
+// Api
+//
+
+//
+b3DynamicBvh::b3DynamicBvh()
+{
+	m_root = 0;
+	m_free = 0;
+	m_lkhd = -1;
+	m_leaves = 0;
+	m_opath = 0;
+}
+
+//
+b3DynamicBvh::~b3DynamicBvh()
+{
+	clear();
+}
+
+//
+void b3DynamicBvh::clear()
+{
+	if (m_root)
+		b3RecurseDeleteNode(this, m_root);
+	b3AlignedFree(m_free);
+	m_free = 0;
+	m_lkhd = -1;
+	m_stkStack.clear();
+	m_opath = 0;
+}
+
+//
+void b3DynamicBvh::optimizeBottomUp()
+{
+	if (m_root)
+	{
+		b3NodeArray leaves;
+		leaves.reserve(m_leaves);
+		b3FetchLeaves(this, m_root, leaves);
+		b3BottomUp(this, &leaves[0], leaves.size());
+		m_root = leaves[0];
+	}
+}
+
+//
+void b3DynamicBvh::optimizeTopDown(int bu_treshold)
+{
+	if (m_root)
+	{
+		b3NodeArray leaves;
+		leaves.reserve(m_leaves);
+		b3FetchLeaves(this, m_root, leaves);
+		m_root = b3TopDown(this, &leaves[0], leaves.size(), bu_treshold);
+	}
+}
+
+//
+void b3DynamicBvh::optimizeIncremental(int passes)
+{
+	if (passes < 0) passes = m_leaves;
+	if (m_root && (passes > 0))
+	{
+		do
+		{
+			b3DbvtNode* node = m_root;
+			unsigned bit = 0;
+			while (node->isinternal())
+			{
+				node = b3Sort(node, m_root)->childs[(m_opath >> bit) & 1];
+				bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
+			}
+			update(node);
+			++m_opath;
+		} while (--passes);
+	}
+}
+
+//
+b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume, void* data)
+{
+	b3DbvtNode* leaf = b3CreateNode(this, 0, volume, data);
+	b3InsertLeaf(this, m_root, leaf);
+	++m_leaves;
+	return (leaf);
+}
+
+//
+void b3DynamicBvh::update(b3DbvtNode* leaf, int lookahead)
+{
+	b3DbvtNode* root = b3RemoveLeaf(this, leaf);
+	if (root)
+	{
+		if (lookahead >= 0)
+		{
+			for (int i = 0; (i < lookahead) && root->parent; ++i)
+			{
+				root = root->parent;
+			}
+		}
+		else
+			root = m_root;
+	}
+	b3InsertLeaf(this, root, leaf);
+}
+
+//
+void b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume)
+{
+	b3DbvtNode* root = b3RemoveLeaf(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;
+	b3InsertLeaf(this, root, leaf);
+}
+
+//
+bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity, b3Scalar margin)
+{
+	if (leaf->volume.Contain(volume)) return (false);
+	volume.Expand(b3MakeVector3(margin, margin, margin));
+	volume.SignedExpand(velocity);
+	update(leaf, volume);
+	return (true);
+}
+
+//
+bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity)
+{
+	if (leaf->volume.Contain(volume)) return (false);
+	volume.SignedExpand(velocity);
+	update(leaf, volume);
+	return (true);
+}
+
+//
+bool b3DynamicBvh::update(b3DbvtNode* leaf, b3DbvtVolume& volume, b3Scalar margin)
+{
+	if (leaf->volume.Contain(volume)) return (false);
+	volume.Expand(b3MakeVector3(margin, margin, margin));
+	update(leaf, volume);
+	return (true);
+}
+
+//
+void b3DynamicBvh::remove(b3DbvtNode* leaf)
+{
+	b3RemoveLeaf(this, leaf);
+	b3DeleteNode(this, leaf);
+	--m_leaves;
+}
+
+//
+void b3DynamicBvh::write(IWriter* iwriter) const
+{
+	b3DbvtNodeEnumerator 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 b3DbvtNode* 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 b3DynamicBvh::clone(b3DynamicBvh& dest, IClone* iclone) const
+{
+	dest.clear();
+	if (m_root != 0)
+	{
+		b3AlignedObjectArray<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];
+			b3DbvtNode* n = b3CreateNode(&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 b3DynamicBvh::maxdepth(const b3DbvtNode* node)
+{
+	int depth = 0;
+	if (node) b3GetMaxDepth(node, 1, depth);
+	return (depth);
+}
+
+//
+int b3DynamicBvh::countLeaves(const b3DbvtNode* node)
+{
+	if (node->isinternal())
+		return (countLeaves(node->childs[0]) + countLeaves(node->childs[1]));
+	else
+		return (1);
+}
+
+//
+void b3DynamicBvh::extractLeaves(const b3DbvtNode* node, b3AlignedObjectArray<const b3DbvtNode*>& leaves)
+{
+	if (node->isinternal())
+	{
+		extractLeaves(node->childs[0], leaves);
+		extractLeaves(node->childs[1], leaves);
+	}
+	else
+	{
+		leaves.push_back(node);
+	}
+}
+
+//
+#if B3_DBVT_ENABLE_BENCHMARK
+
+#include <stdio.h>
+#include <stdlib.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(b3DbvtVolume): 32 bytes
+sizeof(b3DbvtNode):   44 bytes
+[1] b3DbvtVolume intersections: 3499 ms (-1%)
+[2] b3DbvtVolume merges: 1934 ms (0%)
+[3] b3DynamicBvh::collideTT: 5485 ms (-21%)
+[4] b3DynamicBvh::collideTT self: 2814 ms (-20%)
+[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%)
+[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%)
+[7] b3DynamicBvh::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] b3DbvtVolume 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] b3DbvtVolume select: 3419 ms (0%)
+*/
+
+struct b3DbvtBenchmark
+{
+	struct NilPolicy : b3DynamicBvh::ICollide
+	{
+		NilPolicy() : m_pcount(0), m_depth(-B3_INFINITY), m_checksort(true) {}
+		void Process(const b3DbvtNode*, const b3DbvtNode*) { ++m_pcount; }
+		void Process(const b3DbvtNode*) { ++m_pcount; }
+		void Process(const b3DbvtNode*, b3Scalar 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;
+		b3Scalar m_depth;
+		bool m_checksort;
+	};
+	struct P14 : b3DynamicBvh::ICollide
+	{
+		struct Node
+		{
+			const b3DbvtNode* leaf;
+			b3Scalar depth;
+		};
+		void Process(const b3DbvtNode* leaf, b3Scalar 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);
+		}
+		b3AlignedObjectArray<Node> m_nodes;
+	};
+	struct P15 : b3DynamicBvh::ICollide
+	{
+		struct Node
+		{
+			const b3DbvtNode* leaf;
+			b3Scalar depth;
+		};
+		void Process(const b3DbvtNode* 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);
+		}
+		b3AlignedObjectArray<Node> m_nodes;
+		b3Vector3 m_axis;
+	};
+	static b3Scalar RandUnit()
+	{
+		return (rand() / (b3Scalar)RAND_MAX);
+	}
+	static b3Vector3 RandVector3()
+	{
+		return (b3Vector3(RandUnit(), RandUnit(), RandUnit()));
+	}
+	static b3Vector3 RandVector3(b3Scalar cs)
+	{
+		return (RandVector3() * cs - b3Vector3(cs, cs, cs) / 2);
+	}
+	static b3DbvtVolume RandVolume(b3Scalar cs, b3Scalar eb, b3Scalar es)
+	{
+		return (b3DbvtVolume::FromCE(RandVector3(cs), b3Vector3(eb, eb, eb) + RandVector3() * es));
+	}
+	static b3Transform RandTransform(b3Scalar cs)
+	{
+		b3Transform t;
+		t.setOrigin(RandVector3(cs));
+		t.setRotation(b3Quaternion(RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2, RandUnit() * B3_PI * 2).normalized());
+		return (t);
+	}
+	static void RandTree(b3Scalar cs, b3Scalar eb, b3Scalar es, int leaves, b3DynamicBvh& dbvt)
+	{
+		dbvt.clear();
+		for (int i = 0; i < leaves; ++i)
+		{
+			dbvt.insert(RandVolume(cs, eb, es), 0);
+		}
+	}
+};
+
+void b3DynamicBvh::benchmark()
+{
+	static const b3Scalar cfgVolumeCenterScale = 100;
+	static const b3Scalar cfgVolumeExentsBase = 1;
+	static const b3Scalar cfgVolumeExentsScale = 4;
+	static const int cfgLeaves = 8192;
+	static const bool cfgEnable = true;
+
+	//[1] b3DbvtVolume intersections
+	bool cfgBenchmark1_Enable = cfgEnable;
+	static const int cfgBenchmark1_Iterations = 8;
+	static const int cfgBenchmark1_Reference = 3499;
+	//[2] b3DbvtVolume merges
+	bool cfgBenchmark2_Enable = cfgEnable;
+	static const int cfgBenchmark2_Iterations = 4;
+	static const int cfgBenchmark2_Reference = 1945;
+	//[3] b3DynamicBvh::collideTT
+	bool cfgBenchmark3_Enable = cfgEnable;
+	static const int cfgBenchmark3_Iterations = 512;
+	static const int cfgBenchmark3_Reference = 5485;
+	//[4] b3DynamicBvh::collideTT self
+	bool cfgBenchmark4_Enable = cfgEnable;
+	static const int cfgBenchmark4_Iterations = 512;
+	static const int cfgBenchmark4_Reference = 2814;
+	//[5] b3DynamicBvh::collideTT xform
+	bool cfgBenchmark5_Enable = cfgEnable;
+	static const int cfgBenchmark5_Iterations = 512;
+	static const b3Scalar cfgBenchmark5_OffsetScale = 2;
+	static const int cfgBenchmark5_Reference = 7379;
+	//[6] b3DynamicBvh::collideTT xform,self
+	bool cfgBenchmark6_Enable = cfgEnable;
+	static const int cfgBenchmark6_Iterations = 512;
+	static const b3Scalar cfgBenchmark6_OffsetScale = 2;
+	static const int cfgBenchmark6_Reference = 7270;
+	//[7] b3DynamicBvh::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 b3Scalar 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] b3DbvtVolume 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;
+
+	b3Clock 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(b3DbvtVolume): %u bytes\r\n", sizeof(b3DbvtVolume));
+	printf("\tsizeof(b3DbvtNode):   %u bytes\r\n", sizeof(b3DbvtNode));
+	if (cfgBenchmark1_Enable)
+	{  // Benchmark 1
+		srand(380843);
+		b3AlignedObjectArray<b3DbvtVolume> volumes;
+		b3AlignedObjectArray<bool> results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for (int i = 0; i < cfgLeaves; ++i)
+		{
+			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
+		}
+		printf("[1] b3DbvtVolume 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);
+		b3AlignedObjectArray<b3DbvtVolume> volumes;
+		b3AlignedObjectArray<b3DbvtVolume> results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for (int i = 0; i < cfgLeaves; ++i)
+		{
+			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
+		}
+		printf("[2] b3DbvtVolume 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);
+		b3DynamicBvh dbvt[2];
+		b3DbvtBenchmark::NilPolicy policy;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[3] b3DynamicBvh::collideTT: ");
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark3_Iterations; ++i)
+		{
+			b3DynamicBvh::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);
+		b3DynamicBvh dbvt;
+		b3DbvtBenchmark::NilPolicy policy;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
+		dbvt.optimizeTopDown();
+		printf("[4] b3DynamicBvh::collideTT self: ");
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark4_Iterations; ++i)
+		{
+			b3DynamicBvh::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);
+		b3DynamicBvh dbvt[2];
+		b3AlignedObjectArray<b3Transform> transforms;
+		b3DbvtBenchmark::NilPolicy policy;
+		transforms.resize(cfgBenchmark5_Iterations);
+		for (int i = 0; i < transforms.size(); ++i)
+		{
+			transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark5_OffsetScale);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[0]);
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[5] b3DynamicBvh::collideTT xform: ");
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark5_Iterations; ++i)
+		{
+			b3DynamicBvh::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3Transform> transforms;
+		b3DbvtBenchmark::NilPolicy policy;
+		transforms.resize(cfgBenchmark6_Iterations);
+		for (int i = 0; i < transforms.size(); ++i)
+		{
+			transforms[i] = b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale * cfgBenchmark6_OffsetScale);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
+		dbvt.optimizeTopDown();
+		printf("[6] b3DynamicBvh::collideTT xform,self: ");
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark6_Iterations; ++i)
+		{
+			b3DynamicBvh::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3Vector3> rayorg;
+		b3AlignedObjectArray<b3Vector3> raydir;
+		b3DbvtBenchmark::NilPolicy policy;
+		rayorg.resize(cfgBenchmark7_Iterations);
+		raydir.resize(cfgBenchmark7_Iterations);
+		for (int i = 0; i < rayorg.size(); ++i)
+		{
+			rayorg[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
+			raydir[i] = b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale * 2);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale, cfgLeaves, dbvt);
+		dbvt.optimizeTopDown();
+		printf("[7] b3DynamicBvh::rayTest: ");
+		wallclock.reset();
+		for (int i = 0; i < cfgBenchmark7_Passes; ++i)
+		{
+			for (int j = 0; j < cfgBenchmark7_Iterations; ++j)
+			{
+				b3DynamicBvh::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);
+		b3DynamicBvh dbvt;
+		b3DbvtBenchmark::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(b3DbvtBenchmark::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<const b3DbvtNode*> leaves;
+		b3DbvtBenchmark::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<b3DbvtNode*>(leaves[rand() % cfgLeaves]),
+							b3DbvtBenchmark::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<const b3DbvtNode*> leaves;
+		b3AlignedObjectArray<b3Vector3> vectors;
+		vectors.resize(cfgBenchmark10_Iterations);
+		for (int i = 0; i < vectors.size(); ++i)
+		{
+			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)) * cfgBenchmark10_Scale;
+		}
+		b3DbvtBenchmark::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 b3Vector3& d = vectors[j];
+				b3DbvtNode* l = const_cast<b3DbvtNode*>(leaves[rand() % cfgLeaves]);
+				b3DbvtVolume v = b3DbvtVolume::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);
+		b3DynamicBvh dbvt;
+		b3DbvtBenchmark::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);
+		b3AlignedObjectArray<b3DbvtVolume> volumes;
+		b3AlignedObjectArray<bool> results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for (int i = 0; i < cfgLeaves; ++i)
+		{
+			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
+		}
+		printf("[12] b3DbvtVolume 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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3Vector3> vectors;
+		b3DbvtBenchmark::NilPolicy policy;
+		vectors.resize(cfgBenchmark13_Iterations);
+		for (int i = 0; i < vectors.size(); ++i)
+		{
+			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
+		}
+		b3DbvtBenchmark::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 b3Scalar offset = 0;
+			policy.m_depth = -B3_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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3Vector3> vectors;
+		b3DbvtBenchmark::P14 policy;
+		vectors.resize(cfgBenchmark14_Iterations);
+		for (int i = 0; i < vectors.size(); ++i)
+		{
+			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
+		}
+		b3DbvtBenchmark::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 b3Scalar offset = 0;
+			policy.m_nodes.resize(0);
+			dbvt.collideOCL(dbvt.m_root, &vectors[i], &offset, vectors[i], 1, policy, false);
+			policy.m_nodes.quickSort(b3DbvtBenchmark::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3Vector3> vectors;
+		b3DbvtBenchmark::P15 policy;
+		vectors.resize(cfgBenchmark15_Iterations);
+		for (int i = 0; i < vectors.size(); ++i)
+		{
+			vectors[i] = (b3DbvtBenchmark::RandVector3() * 2 - b3Vector3(1, 1, 1)).normalized();
+		}
+		b3DbvtBenchmark::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 b3Scalar 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(b3DbvtBenchmark::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);
+		b3DynamicBvh dbvt;
+		b3AlignedObjectArray<b3DbvtNode*> batch;
+		b3DbvtBenchmark::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(b3DbvtBenchmark::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);
+		b3AlignedObjectArray<b3DbvtVolume> volumes;
+		b3AlignedObjectArray<int> results;
+		b3AlignedObjectArray<int> indices;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		indices.resize(cfgLeaves);
+		for (int i = 0; i < cfgLeaves; ++i)
+		{
+			indices[i] = i;
+			volumes[i] = b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale, cfgVolumeExentsBase, cfgVolumeExentsScale);
+		}
+		for (int i = 0; i < cfgLeaves; ++i)
+		{
+			b3Swap(indices[i], indices[rand() % cfgLeaves]);
+		}
+		printf("[17] b3DbvtVolume 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

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h

@@ -0,0 +1,1332 @@
+/*
+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.
+*/
+///b3DynamicBvh implementation by Nathanael Presson
+
+#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+
+//
+// Compile time configuration
+//
+
+// Implementation profiles
+#define B3_DBVT_IMPL_GENERIC 0  // Generic implementation
+#define B3_DBVT_IMPL_SSE 1      // SSE
+
+// Template implementation of ICollide
+#ifdef _WIN32
+#if (defined(_MSC_VER) && _MSC_VER >= 1400)
+#define B3_DBVT_USE_TEMPLATE 1
+#else
+#define B3_DBVT_USE_TEMPLATE 0
+#endif
+#else
+#define B3_DBVT_USE_TEMPLATE 0
+#endif
+
+// Use only intrinsics instead of inline asm
+#define B3_DBVT_USE_INTRINSIC_SSE 1
+
+// Using memmov for collideOCL
+#define B3_DBVT_USE_MEMMOVE 1
+
+// Enable benchmarking code
+#define B3_DBVT_ENABLE_BENCHMARK 0
+
+// Inlining
+#define B3_DBVT_INLINE B3_FORCE_INLINE
+
+// Specific methods implementation
+
+//SSE gives errors on a MSVC 7.1
+#if defined(B3_USE_SSE)  //&& defined (_WIN32)
+#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_SSE
+#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_SSE
+#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_SSE
+#else
+#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_GENERIC
+#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_GENERIC
+#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_GENERIC
+#endif
+
+#if (B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE) || \
+	(B3_DBVT_MERGE_IMPL == B3_DBVT_IMPL_SSE) ||  \
+	(B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE)
+#include <emmintrin.h>
+#endif
+
+//
+// Auto config and checks
+//
+
+#if B3_DBVT_USE_TEMPLATE
+#define B3_DBVT_VIRTUAL
+#define B3_DBVT_VIRTUAL_DTOR(a)
+#define B3_DBVT_PREFIX template <typename T>
+#define B3_DBVT_IPOLICY T& policy
+#define B3_DBVT_CHECKTYPE                        \
+	static const ICollide& typechecker = *(T*)1; \
+	(void)typechecker;
+#else
+#define B3_DBVT_VIRTUAL_DTOR(a) \
+	virtual ~a() {}
+#define B3_DBVT_VIRTUAL virtual
+#define B3_DBVT_PREFIX
+#define B3_DBVT_IPOLICY ICollide& policy
+#define B3_DBVT_CHECKTYPE
+#endif
+
+#if B3_DBVT_USE_MEMMOVE
+#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
+#include <memory.h>
+#endif
+#include <string.h>
+#endif
+
+#ifndef B3_DBVT_USE_TEMPLATE
+#error "B3_DBVT_USE_TEMPLATE undefined"
+#endif
+
+#ifndef B3_DBVT_USE_MEMMOVE
+#error "B3_DBVT_USE_MEMMOVE undefined"
+#endif
+
+#ifndef B3_DBVT_ENABLE_BENCHMARK
+#error "B3_DBVT_ENABLE_BENCHMARK undefined"
+#endif
+
+#ifndef B3_DBVT_SELECT_IMPL
+#error "B3_DBVT_SELECT_IMPL undefined"
+#endif
+
+#ifndef B3_DBVT_MERGE_IMPL
+#error "B3_DBVT_MERGE_IMPL undefined"
+#endif
+
+#ifndef B3_DBVT_INT0_IMPL
+#error "B3_DBVT_INT0_IMPL undefined"
+#endif
+
+//
+// Defaults volumes
+//
+
+/* b3DbvtAabbMm			*/
+struct b3DbvtAabbMm
+{
+	B3_DBVT_INLINE b3Vector3 Center() const { return ((mi + mx) / 2); }
+	B3_DBVT_INLINE b3Vector3 Lengths() const { return (mx - mi); }
+	B3_DBVT_INLINE b3Vector3 Extents() const { return ((mx - mi) / 2); }
+	B3_DBVT_INLINE const b3Vector3& Mins() const { return (mi); }
+	B3_DBVT_INLINE const b3Vector3& Maxs() const { return (mx); }
+	static inline b3DbvtAabbMm FromCE(const b3Vector3& c, const b3Vector3& e);
+	static inline b3DbvtAabbMm FromCR(const b3Vector3& c, b3Scalar r);
+	static inline b3DbvtAabbMm FromMM(const b3Vector3& mi, const b3Vector3& mx);
+	static inline b3DbvtAabbMm FromPoints(const b3Vector3* pts, int n);
+	static inline b3DbvtAabbMm FromPoints(const b3Vector3** ppts, int n);
+	B3_DBVT_INLINE void Expand(const b3Vector3& e);
+	B3_DBVT_INLINE void SignedExpand(const b3Vector3& e);
+	B3_DBVT_INLINE bool Contain(const b3DbvtAabbMm& a) const;
+	B3_DBVT_INLINE int Classify(const b3Vector3& n, b3Scalar o, int s) const;
+	B3_DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v, unsigned signs) const;
+	B3_DBVT_INLINE friend bool b3Intersect(const b3DbvtAabbMm& a,
+										   const b3DbvtAabbMm& b);
+
+	B3_DBVT_INLINE friend bool b3Intersect(const b3DbvtAabbMm& a,
+										   const b3Vector3& b);
+
+	B3_DBVT_INLINE friend b3Scalar b3Proximity(const b3DbvtAabbMm& a,
+											   const b3DbvtAabbMm& b);
+	B3_DBVT_INLINE friend int b3Select(const b3DbvtAabbMm& o,
+									   const b3DbvtAabbMm& a,
+									   const b3DbvtAabbMm& b);
+	B3_DBVT_INLINE friend void b3Merge(const b3DbvtAabbMm& a,
+									   const b3DbvtAabbMm& b,
+									   b3DbvtAabbMm& r);
+	B3_DBVT_INLINE friend bool b3NotEqual(const b3DbvtAabbMm& a,
+										  const b3DbvtAabbMm& b);
+
+	B3_DBVT_INLINE b3Vector3& tMins() { return (mi); }
+	B3_DBVT_INLINE b3Vector3& tMaxs() { return (mx); }
+
+private:
+	B3_DBVT_INLINE void AddSpan(const b3Vector3& d, b3Scalar& smi, b3Scalar& smx) const;
+
+private:
+	b3Vector3 mi, mx;
+};
+
+// Types
+typedef b3DbvtAabbMm b3DbvtVolume;
+
+/* b3DbvtNode				*/
+struct b3DbvtNode
+{
+	b3DbvtVolume volume;
+	b3DbvtNode* parent;
+	B3_DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
+	B3_DBVT_INLINE bool isinternal() const { return (!isleaf()); }
+	union {
+		b3DbvtNode* childs[2];
+		void* data;
+		int dataAsInt;
+	};
+};
+
+///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes.
+///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
+struct b3DynamicBvh
+{
+	/* Stack element	*/
+	struct sStkNN
+	{
+		const b3DbvtNode* a;
+		const b3DbvtNode* b;
+		sStkNN() {}
+		sStkNN(const b3DbvtNode* na, const b3DbvtNode* nb) : a(na), b(nb) {}
+	};
+	struct sStkNP
+	{
+		const b3DbvtNode* node;
+		int mask;
+		sStkNP(const b3DbvtNode* n, unsigned m) : node(n), mask(m) {}
+	};
+	struct sStkNPS
+	{
+		const b3DbvtNode* node;
+		int mask;
+		b3Scalar value;
+		sStkNPS() {}
+		sStkNPS(const b3DbvtNode* n, unsigned m, b3Scalar v) : node(n), mask(m), value(v) {}
+	};
+	struct sStkCLN
+	{
+		const b3DbvtNode* node;
+		b3DbvtNode* parent;
+		sStkCLN(const b3DbvtNode* n, b3DbvtNode* p) : node(n), parent(p) {}
+	};
+	// Policies/Interfaces
+
+	/* ICollide	*/
+	struct ICollide
+	{
+		B3_DBVT_VIRTUAL_DTOR(ICollide)
+		B3_DBVT_VIRTUAL void Process(const b3DbvtNode*, const b3DbvtNode*) {}
+		B3_DBVT_VIRTUAL void Process(const b3DbvtNode*) {}
+		B3_DBVT_VIRTUAL void Process(const b3DbvtNode* n, b3Scalar) { Process(n); }
+		B3_DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return (true); }
+		B3_DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return (true); }
+	};
+	/* IWriter	*/
+	struct IWriter
+	{
+		virtual ~IWriter() {}
+		virtual void Prepare(const b3DbvtNode* root, int numnodes) = 0;
+		virtual void WriteNode(const b3DbvtNode*, int index, int parent, int child0, int child1) = 0;
+		virtual void WriteLeaf(const b3DbvtNode*, int index, int parent) = 0;
+	};
+	/* IClone	*/
+	struct IClone
+	{
+		virtual ~IClone() {}
+		virtual void CloneLeaf(b3DbvtNode*) {}
+	};
+
+	// Constants
+	enum
+	{
+		B3_SIMPLE_STACKSIZE = 64,
+		B3_DOUBLE_STACKSIZE = B3_SIMPLE_STACKSIZE * 2
+	};
+
+	// Fields
+	b3DbvtNode* m_root;
+	b3DbvtNode* m_free;
+	int m_lkhd;
+	int m_leaves;
+	unsigned m_opath;
+
+	b3AlignedObjectArray<sStkNN> m_stkStack;
+	mutable b3AlignedObjectArray<const b3DbvtNode*> m_rayTestStack;
+
+	// Methods
+	b3DynamicBvh();
+	~b3DynamicBvh();
+	void clear();
+	bool empty() const { return (0 == m_root); }
+	void optimizeBottomUp();
+	void optimizeTopDown(int bu_treshold = 128);
+	void optimizeIncremental(int passes);
+	b3DbvtNode* insert(const b3DbvtVolume& box, void* data);
+	void update(b3DbvtNode* leaf, int lookahead = -1);
+	void update(b3DbvtNode* leaf, b3DbvtVolume& volume);
+	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity, b3Scalar margin);
+	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, const b3Vector3& velocity);
+	bool update(b3DbvtNode* leaf, b3DbvtVolume& volume, b3Scalar margin);
+	void remove(b3DbvtNode* leaf);
+	void write(IWriter* iwriter) const;
+	void clone(b3DynamicBvh& dest, IClone* iclone = 0) const;
+	static int maxdepth(const b3DbvtNode* node);
+	static int countLeaves(const b3DbvtNode* node);
+	static void extractLeaves(const b3DbvtNode* node, b3AlignedObjectArray<const b3DbvtNode*>& leaves);
+#if B3_DBVT_ENABLE_BENCHMARK
+	static void benchmark();
+#else
+	static void benchmark()
+	{
+	}
+#endif
+	// B3_DBVT_IPOLICY must support ICollide policy/interface
+	B3_DBVT_PREFIX
+	static void enumNodes(const b3DbvtNode* root,
+						  B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+	static void enumLeaves(const b3DbvtNode* root,
+						   B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+	void collideTT(const b3DbvtNode* root0,
+				   const b3DbvtNode* root1,
+				   B3_DBVT_IPOLICY);
+
+	B3_DBVT_PREFIX
+	void collideTTpersistentStack(const b3DbvtNode* root0,
+								  const b3DbvtNode* root1,
+								  B3_DBVT_IPOLICY);
+#if 0
+	B3_DBVT_PREFIX
+		void		collideTT(	const b3DbvtNode* root0,
+		const b3DbvtNode* root1,
+		const b3Transform& xform,
+		B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+		void		collideTT(	const b3DbvtNode* root0,
+		const b3Transform& xform0,
+		const b3DbvtNode* root1,
+		const b3Transform& xform1,
+		B3_DBVT_IPOLICY);
+#endif
+
+	B3_DBVT_PREFIX
+	void collideTV(const b3DbvtNode* root,
+				   const b3DbvtVolume& volume,
+				   B3_DBVT_IPOLICY) const;
+	///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc 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
+	B3_DBVT_PREFIX
+	static void rayTest(const b3DbvtNode* root,
+						const b3Vector3& rayFrom,
+						const b3Vector3& rayTo,
+						B3_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 b3DynamicBvhBroadphase to accelerate world ray casts
+	B3_DBVT_PREFIX
+	void rayTestInternal(const b3DbvtNode* root,
+						 const b3Vector3& rayFrom,
+						 const b3Vector3& rayTo,
+						 const b3Vector3& rayDirectionInverse,
+						 unsigned int signs[3],
+						 b3Scalar lambda_max,
+						 const b3Vector3& aabbMin,
+						 const b3Vector3& aabbMax,
+						 B3_DBVT_IPOLICY) const;
+
+	B3_DBVT_PREFIX
+	static void collideKDOP(const b3DbvtNode* root,
+							const b3Vector3* normals,
+							const b3Scalar* offsets,
+							int count,
+							B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+	static void collideOCL(const b3DbvtNode* root,
+						   const b3Vector3* normals,
+						   const b3Scalar* offsets,
+						   const b3Vector3& sortaxis,
+						   int count,
+						   B3_DBVT_IPOLICY,
+						   bool fullsort = true);
+	B3_DBVT_PREFIX
+	static void collideTU(const b3DbvtNode* root,
+						  B3_DBVT_IPOLICY);
+	// Helpers
+	static B3_DBVT_INLINE int nearest(const int* i, const b3DynamicBvh::sStkNPS* a, b3Scalar 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 B3_DBVT_INLINE int allocate(b3AlignedObjectArray<int>& ifree,
+									   b3AlignedObjectArray<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:
+	b3DynamicBvh(const b3DynamicBvh&) {}
+};
+
+//
+// Inline's
+//
+
+//
+inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c, const b3Vector3& e)
+{
+	b3DbvtAabbMm box;
+	box.mi = c - e;
+	box.mx = c + e;
+	return (box);
+}
+
+//
+inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c, b3Scalar r)
+{
+	return (FromCE(c, b3MakeVector3(r, r, r)));
+}
+
+//
+inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi, const b3Vector3& mx)
+{
+	b3DbvtAabbMm box;
+	box.mi = mi;
+	box.mx = mx;
+	return (box);
+}
+
+//
+inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3* pts, int n)
+{
+	b3DbvtAabbMm 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 b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts, int n)
+{
+	b3DbvtAabbMm 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);
+}
+
+//
+B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e)
+{
+	mi -= e;
+	mx += e;
+}
+
+//
+B3_DBVT_INLINE void b3DbvtAabbMm::SignedExpand(const b3Vector3& 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]);
+}
+
+//
+B3_DBVT_INLINE bool b3DbvtAabbMm::Contain(const b3DbvtAabbMm& 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));
+}
+
+//
+B3_DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n, b3Scalar o, int s) const
+{
+	b3Vector3 pi, px;
+	switch (s)
+	{
+		case (0 + 0 + 0):
+			px = b3MakeVector3(mi.x, mi.y, mi.z);
+			pi = b3MakeVector3(mx.x, mx.y, mx.z);
+			break;
+		case (1 + 0 + 0):
+			px = b3MakeVector3(mx.x, mi.y, mi.z);
+			pi = b3MakeVector3(mi.x, mx.y, mx.z);
+			break;
+		case (0 + 2 + 0):
+			px = b3MakeVector3(mi.x, mx.y, mi.z);
+			pi = b3MakeVector3(mx.x, mi.y, mx.z);
+			break;
+		case (1 + 2 + 0):
+			px = b3MakeVector3(mx.x, mx.y, mi.z);
+			pi = b3MakeVector3(mi.x, mi.y, mx.z);
+			break;
+		case (0 + 0 + 4):
+			px = b3MakeVector3(mi.x, mi.y, mx.z);
+			pi = b3MakeVector3(mx.x, mx.y, mi.z);
+			break;
+		case (1 + 0 + 4):
+			px = b3MakeVector3(mx.x, mi.y, mx.z);
+			pi = b3MakeVector3(mi.x, mx.y, mi.z);
+			break;
+		case (0 + 2 + 4):
+			px = b3MakeVector3(mi.x, mx.y, mx.z);
+			pi = b3MakeVector3(mx.x, mi.y, mi.z);
+			break;
+		case (1 + 2 + 4):
+			px = b3MakeVector3(mx.x, mx.y, mx.z);
+			pi = b3MakeVector3(mi.x, mi.y, mi.z);
+			break;
+	}
+	if ((b3Dot(n, px) + o) < 0) return (-1);
+	if ((b3Dot(n, pi) + o) >= 0) return (+1);
+	return (0);
+}
+
+//
+B3_DBVT_INLINE b3Scalar b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v, unsigned signs) const
+{
+	const b3Vector3* b[] = {&mx, &mi};
+	const b3Vector3 p = b3MakeVector3(b[(signs >> 0) & 1]->x,
+									  b[(signs >> 1) & 1]->y,
+									  b[(signs >> 2) & 1]->z);
+	return (b3Dot(p, v));
+}
+
+//
+B3_DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d, b3Scalar& smi, b3Scalar& 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];
+		}
+	}
+}
+
+//
+B3_DBVT_INLINE bool b3Intersect(const b3DbvtAabbMm& a,
+								const b3DbvtAabbMm& b)
+{
+#if B3_DBVT_INT0_IMPL == B3_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
+}
+
+//
+B3_DBVT_INLINE bool b3Intersect(const b3DbvtAabbMm& a,
+								const b3Vector3& 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));
+}
+
+//////////////////////////////////////
+
+//
+B3_DBVT_INLINE b3Scalar b3Proximity(const b3DbvtAabbMm& a,
+									const b3DbvtAabbMm& b)
+{
+	const b3Vector3 d = (a.mi + a.mx) - (b.mi + b.mx);
+	return (b3Fabs(d.x) + b3Fabs(d.y) + b3Fabs(d.z));
+}
+
+//
+B3_DBVT_INLINE int b3Select(const b3DbvtAabbMm& o,
+							const b3DbvtAabbMm& a,
+							const b3DbvtAabbMm& b)
+{
+#if B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE
+
+#if defined(_WIN32)
+	static B3_ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
+#else
+	static B3_ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/};
+#endif
+	///@todo: the intrinsic version is 11% slower
+#if B3_DBVT_USE_INTRINSIC_SSE
+
+	union b3SSEUnion  ///NOTE: if we use more intrinsics, move b3SSEUnion 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));
+
+	b3SSEUnion tmp;
+	tmp.ssereg = _mm_cmple_ss(bmi, ami);
+	return tmp.ints[0] & 1;
+
+#else
+	B3_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 (b3Proximity(o, a) < b3Proximity(o, b) ? 0 : 1);
+#endif
+}
+
+//
+B3_DBVT_INLINE void b3Merge(const b3DbvtAabbMm& a,
+							const b3DbvtAabbMm& b,
+							b3DbvtAabbMm& r)
+{
+#if B3_DBVT_MERGE_IMPL == B3_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
+}
+
+//
+B3_DBVT_INLINE bool b3NotEqual(const b3DbvtAabbMm& a,
+							   const b3DbvtAabbMm& 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
+//
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::enumNodes(const b3DbvtNode* root,
+									B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	policy.Process(root);
+	if (root->isinternal())
+	{
+		enumNodes(root->childs[0], policy);
+		enumNodes(root->childs[1], policy);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::enumLeaves(const b3DbvtNode* root,
+									 B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root->isinternal())
+	{
+		enumLeaves(root->childs[0], policy);
+		enumLeaves(root->childs[1], policy);
+	}
+	else
+	{
+		policy.Process(root);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideTT(const b3DbvtNode* root0,
+									const b3DbvtNode* root1,
+									B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root0 && root1)
+	{
+		int depth = 1;
+		int treshold = B3_DOUBLE_STACKSIZE - 4;
+		b3AlignedObjectArray<sStkNN> stkStack;
+		stkStack.resize(B3_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 (b3Intersect(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);
+	}
+}
+
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideTTpersistentStack(const b3DbvtNode* root0,
+												   const b3DbvtNode* root1,
+												   B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root0 && root1)
+	{
+		int depth = 1;
+		int treshold = B3_DOUBLE_STACKSIZE - 4;
+
+		m_stkStack.resize(B3_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 (b3Intersect(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
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
+								  const b3DbvtNode* root1,
+								  const b3Transform& xform,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=B3_DOUBLE_STACKSIZE-4;
+			b3AlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(B3_DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{
+				sStkNN	p=stkStack[--depth];
+				if(b3Intersect(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);
+		}
+}
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
+								  const b3Transform& xform0,
+								  const b3DbvtNode* root1,
+								  const b3Transform& xform1,
+								  B3_DBVT_IPOLICY)
+{
+	const b3Transform	xform=xform0.inverse()*xform1;
+	collideTT(root0,root1,xform,policy);
+}
+#endif
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideTV(const b3DbvtNode* root,
+									const b3DbvtVolume& vol,
+									B3_DBVT_IPOLICY) const
+{
+	B3_DBVT_CHECKTYPE
+	if (root)
+	{
+		B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+		volume(vol);
+		b3AlignedObjectArray<const b3DbvtNode*> stack;
+		stack.resize(0);
+		stack.reserve(B3_SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do
+		{
+			const b3DbvtNode* n = stack[stack.size() - 1];
+			stack.pop_back();
+			if (b3Intersect(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);
+	}
+}
+
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::rayTestInternal(const b3DbvtNode* root,
+										  const b3Vector3& rayFrom,
+										  const b3Vector3& rayTo,
+										  const b3Vector3& rayDirectionInverse,
+										  unsigned int signs[3],
+										  b3Scalar lambda_max,
+										  const b3Vector3& aabbMin,
+										  const b3Vector3& aabbMax,
+										  B3_DBVT_IPOLICY) const
+{
+	(void)rayTo;
+	B3_DBVT_CHECKTYPE
+	if (root)
+	{
+		int depth = 1;
+		int treshold = B3_DOUBLE_STACKSIZE - 2;
+		b3AlignedObjectArray<const b3DbvtNode*>& stack = m_rayTestStack;
+		stack.resize(B3_DOUBLE_STACKSIZE);
+		stack[0] = root;
+		b3Vector3 bounds[2];
+		do
+		{
+			const b3DbvtNode* node = stack[--depth];
+			bounds[0] = node->volume.Mins() - aabbMax;
+			bounds[1] = node->volume.Maxs() - aabbMin;
+			b3Scalar tmin = 1.f, lambda_min = 0.f;
+			unsigned int result1 = false;
+			result1 = b3RayAabb2(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);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::rayTest(const b3DbvtNode* root,
+								  const b3Vector3& rayFrom,
+								  const b3Vector3& rayTo,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root)
+	{
+		b3Vector3 rayDir = (rayTo - rayFrom);
+		rayDir.normalize();
+
+		///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
+		b3Vector3 rayDirectionInverse;
+		rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0];
+		rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1];
+		rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2];
+		unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+
+		b3Scalar lambda_max = rayDir.dot(rayTo - rayFrom);
+#ifdef COMPARE_BTRAY_AABB2
+		b3Vector3 resultNormal;
+#endif  //COMPARE_BTRAY_AABB2
+
+		b3AlignedObjectArray<const b3DbvtNode*> stack;
+
+		int depth = 1;
+		int treshold = B3_DOUBLE_STACKSIZE - 2;
+
+		stack.resize(B3_DOUBLE_STACKSIZE);
+		stack[0] = root;
+		b3Vector3 bounds[2];
+		do
+		{
+			const b3DbvtNode* node = stack[--depth];
+
+			bounds[0] = node->volume.Mins();
+			bounds[1] = node->volume.Maxs();
+
+			b3Scalar tmin = 1.f, lambda_min = 0.f;
+			unsigned int result1 = b3RayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
+
+#ifdef COMPARE_BTRAY_AABB2
+			b3Scalar param = 1.f;
+			bool result2 = b3RayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal);
+			b3Assert(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);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
+									  const b3Vector3* normals,
+									  const b3Scalar* offsets,
+									  int count,
+									  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root)
+	{
+		const int inside = (1 << count) - 1;
+		b3AlignedObjectArray<sStkNP> stack;
+		int signs[sizeof(unsigned) * 8];
+		b3Assert(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(B3_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());
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideOCL(const b3DbvtNode* root,
+									 const b3Vector3* normals,
+									 const b3Scalar* offsets,
+									 const b3Vector3& sortaxis,
+									 int count,
+									 B3_DBVT_IPOLICY,
+									 bool fsort)
+{
+	B3_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;
+		b3AlignedObjectArray<sStkNPS> stock;
+		b3AlignedObjectArray<int> ifree;
+		b3AlignedObjectArray<int> stack;
+		int signs[sizeof(unsigned) * 8];
+		b3Assert(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(B3_SIMPLE_STACKSIZE);
+		stack.reserve(B3_SIMPLE_STACKSIZE);
+		ifree.reserve(B3_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 b3DbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]};
+					sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)),
+									 sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))};
+					const int q = nes[0].value < nes[1].value ? 1 : 0;
+					int j = stack.size();
+					if (fsort && (j > 0))
+					{
+						/* Insert 0	*/
+						j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size());
+						stack.push_back(0);
+#if B3_DBVT_USE_MEMMOVE
+						memmove(&stack[j + 1], &stack[j], sizeof(int) * (stack.size() - j - 1));
+#else
+						for (int k = stack.size() - 1; k > j; --k) stack[k] = stack[k - 1];
+#endif
+						stack[j] = allocate(ifree, stock, nes[q]);
+						/* Insert 1	*/
+						j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size());
+						stack.push_back(0);
+#if B3_DBVT_USE_MEMMOVE
+						memmove(&stack[j + 1], &stack[j], sizeof(int) * (stack.size() - j - 1));
+#else
+						for (int k = stack.size() - 1; k > j; --k) stack[k] = stack[k - 1];
+#endif
+						stack[j] = allocate(ifree, stock, nes[1 - q]);
+					}
+					else
+					{
+						stack.push_back(allocate(ifree, stock, nes[q]));
+						stack.push_back(allocate(ifree, stock, nes[1 - q]));
+					}
+				}
+				else
+				{
+					policy.Process(se.node, se.value);
+				}
+			}
+		} while (stack.size());
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void b3DynamicBvh::collideTU(const b3DbvtNode* root,
+									B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+	if (root)
+	{
+		b3AlignedObjectArray<const b3DbvtNode*> stack;
+		stack.reserve(B3_SIMPLE_STACKSIZE);
+		stack.push_back(root);
+		do
+		{
+			const b3DbvtNode* 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 B3_DBVT_USE_MEMMOVE
+#undef B3_DBVT_USE_TEMPLATE
+#undef B3_DBVT_VIRTUAL_DTOR
+#undef B3_DBVT_VIRTUAL
+#undef B3_DBVT_PREFIX
+#undef B3_DBVT_IPOLICY
+#undef B3_DBVT_CHECKTYPE
+#undef B3_DBVT_IMPL_GENERIC
+#undef B3_DBVT_IMPL_SSE
+#undef B3_DBVT_USE_INTRINSIC_SSE
+#undef B3_DBVT_SELECT_IMPL
+#undef B3_DBVT_MERGE_IMPL
+#undef B3_DBVT_INT0_IMPL
+
+#endif

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp

@@ -0,0 +1,808 @@
+/*
+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.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+
+#include "b3DynamicBvhBroadphase.h"
+#include "b3OverlappingPair.h"
+
+//
+// Profiling
+//
+
+#if B3_DBVT_BP_PROFILE || B3_DBVT_BP_ENABLE_BENCHMARK
+#include <stdio.h>
+#endif
+
+#if B3_DBVT_BP_PROFILE
+struct b3ProfileScope
+{
+	__forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
+	{
+	}
+	__forceinline ~b3ProfileScope()
+	{
+		(*m_value) += m_clock->getTimeMicroseconds() - m_base;
+	}
+	b3Clock* m_clock;
+	unsigned long* m_value;
+	unsigned long m_base;
+};
+#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock, _value_)
+#else
+#define b3SPC(_value_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void b3ListAppend(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 b3ListRemove(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 b3ListCount(T* root)
+{
+	int n = 0;
+	while (root)
+	{
+		++n;
+		root = root->links[1];
+	}
+	return (n);
+}
+
+//
+template <typename T>
+static inline void b3Clear(T& value)
+{
+	static const struct ZeroDummy : T
+	{
+	} zerodummy;
+	value = zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider	*/
+struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
+{
+	b3DynamicBvhBroadphase* pbp;
+	b3DbvtProxy* proxy;
+	b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
+	void Process(const b3DbvtNode* na, const b3DbvtNode* nb)
+	{
+		if (na != nb)
+		{
+			b3DbvtProxy* pa = (b3DbvtProxy*)na->data;
+			b3DbvtProxy* pb = (b3DbvtProxy*)nb->data;
+#if B3_DBVT_BP_SORTPAIRS
+			if (pa->m_uniqueId > pb->m_uniqueId)
+				b3Swap(pa, pb);
+#endif
+			pbp->m_paircache->addOverlappingPair(pa->getUid(), pb->getUid());
+			++pbp->m_newpairs;
+		}
+	}
+	void Process(const b3DbvtNode* n)
+	{
+		Process(n, proxy->leaf);
+	}
+};
+
+//
+// b3DynamicBvhBroadphase
+//
+
+//
+b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* 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 (b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache), 16)) b3HashedOverlappingPairCache();
+
+	m_pid = 0;
+	m_cid = 0;
+	for (int i = 0; i <= STAGECOUNT; ++i)
+	{
+		m_stageRoots[i] = 0;
+	}
+#if B3_DBVT_BP_PROFILE
+	b3Clear(m_profiling);
+#endif
+	m_proxies.resize(proxyCapacity);
+}
+
+//
+b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
+{
+	if (m_releasepaircache)
+	{
+		m_paircache->~b3OverlappingPairCache();
+		b3AlignedFree(m_paircache);
+	}
+}
+
+//
+b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy(const b3Vector3& aabbMin,
+													   const b3Vector3& aabbMax,
+													   int objectId,
+													   void* userPtr,
+													   int collisionFilterGroup,
+													   int collisionFilterMask)
+{
+	b3DbvtProxy* mem = &m_proxies[objectId];
+	b3DbvtProxy* proxy = new (mem) b3DbvtProxy(aabbMin, aabbMax, userPtr,
+											   collisionFilterGroup,
+											   collisionFilterMask);
+
+	b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+
+	//bproxy->aabb			=	b3DbvtVolume::FromMM(aabbMin,aabbMax);
+	proxy->stage = m_stageCurrent;
+	proxy->m_uniqueId = objectId;
+	proxy->leaf = m_sets[0].insert(aabb, proxy);
+	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+	if (!m_deferedcollide)
+	{
+		b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::destroyProxy(b3BroadphaseProxy* absproxy,
+										  b3Dispatcher* dispatcher)
+{
+	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
+	if (proxy->stage == STAGECOUNT)
+		m_sets[1].remove(proxy->leaf);
+	else
+		m_sets[0].remove(proxy->leaf);
+	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+	m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(), dispatcher);
+
+	m_needcleanup = true;
+}
+
+void b3DynamicBvhBroadphase::getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const
+{
+	const b3DbvtProxy* proxy = &m_proxies[objectId];
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+/*
+void	b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
+{
+	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+*/
+
+struct BroadphaseRayTester : b3DynamicBvh::ICollide
+{
+	b3BroadphaseRayCallback& m_rayCallback;
+	BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
+		: m_rayCallback(orgCallback)
+	{
+	}
+	void Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
+		m_rayCallback.process(proxy);
+	}
+};
+
+void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin, const b3Vector3& 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 : b3DynamicBvh::ICollide
+{
+	b3BroadphaseAabbCallback& m_aabbCallback;
+	BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
+		: m_aabbCallback(orgCallback)
+	{
+	}
+	void Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
+		m_aabbCallback.process(proxy);
+	}
+};
+
+void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& aabbCallback)
+{
+	BroadphaseAabbTester callback(aabbCallback);
+
+	const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds = b3DbvtVolume::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 b3DynamicBvhBroadphase::setAabb(int objectId,
+									 const b3Vector3& aabbMin,
+									 const b3Vector3& aabbMax,
+									 b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy* proxy = &m_proxies[objectId];
+	//	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+#if B3_DBVT_BP_PREVENTFALSEUPDATE
+	if (b3NotEqual(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 (b3Intersect(proxy->leaf->volume, aabb))
+			{ /* Moving				*/
+
+				const b3Vector3 delta = aabbMin - proxy->m_aabbMin;
+				b3Vector3 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 B3_DBVT_BP_MARGIN
+					m_sets[0].update(proxy->leaf, aabb, velocity, B3_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;
+			}
+		}
+		b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+		proxy->m_aabbMin = aabbMin;
+		proxy->m_aabbMax = aabbMax;
+		proxy->stage = m_stageCurrent;
+		b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+		if (docollide)
+		{
+			m_needcleanup = true;
+			if (!m_deferedcollide)
+			{
+				b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
+												const b3Vector3& aabbMin,
+												const b3Vector3& aabbMax,
+												b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	aabb = b3DbvtVolume::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;
+	}
+	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+	proxy->m_aabbMin = aabbMin;
+	proxy->m_aabbMax = aabbMax;
+	proxy->stage = m_stageCurrent;
+	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+	if (docollide)
+	{
+		m_needcleanup = true;
+		if (!m_deferedcollide)
+		{
+			b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	collide(dispatcher);
+#if B3_DBVT_BP_PROFILE
+	if (0 == (m_pid % B3_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 / B3_DBVT_BP_PROFILING_RATE);
+		printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / B3_DBVT_BP_PROFILING_RATE);
+		printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / B3_DBVT_BP_PROFILING_RATE);
+		printf("total:     %uus\r\n", total / B3_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) / B3_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) * B3_DBVT_BP_PROFILING_RATE));
+		b3Clear(m_profiling);
+		m_clock.reset();
+	}
+#endif
+
+	performDeferredRemoval(dispatcher);
+}
+
+void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
+{
+	if (m_paircache->hasDeferredRemoval())
+	{
+		b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+
+		int invalidPair = 0;
+
+		int i;
+
+		b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
+
+		for (i = 0; i < overlappingPairArray.size(); i++)
+		{
+			b3BroadphasePair& 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
+				b3DbvtProxy* pa = &m_proxies[pair.x];
+				b3DbvtProxy* pb = &m_proxies[pair.y];
+				bool hasOverlap = b3Intersect(pa->leaf->volume, pb->leaf->volume);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				}
+				else
+				{
+					needsRemoval = true;
+				}
+			}
+			else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+			}
+
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair, dispatcher);
+
+				pair.x = -1;
+				pair.y = -1;
+				invalidPair++;
+			}
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::collide(b3Dispatcher* 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");
+	}
+*/
+
+	b3SPC(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 = b3Max<int>(0, m_fixedleft - count);
+	}
+	/* dynamic -> fixed set	*/
+	m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
+	b3DbvtProxy* current = m_stageRoots[m_stageCurrent];
+	if (current)
+	{
+		b3DbvtTreeCollider collider(this);
+		do
+		{
+			b3DbvtProxy* next = current->links[1];
+			b3ListRemove(current, m_stageRoots[current->stage]);
+			b3ListAppend(current, m_stageRoots[STAGECOUNT]);
+#if B3_DBVT_BP_ACCURATESLEEPING
+			m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
+			collider.proxy = current;
+			b3DynamicBvh::collideTV(m_sets[0].m_root, current->aabb, collider);
+			b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
+#endif
+			m_sets[0].remove(current->leaf);
+			B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+			curAabb = b3DbvtVolume::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		*/
+	{
+		b3DbvtTreeCollider collider(this);
+		if (m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_fdcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
+		}
+		if (m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_ddcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
+		}
+	}
+	/* clean up				*/
+	if (m_needcleanup)
+	{
+		b3SPC(m_profiling.m_cleanup);
+		b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
+		if (pairs.size() > 0)
+		{
+			int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
+			for (int i = 0; i < ni; ++i)
+			{
+				b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
+				b3DbvtProxy* pa = &m_proxies[p.x];
+				b3DbvtProxy* pb = &m_proxies[p.y];
+				if (!b3Intersect(pa->leaf->volume, pb->leaf->volume))
+				{
+#if B3_DBVT_BP_SORTPAIRS
+					if (pa->m_uniqueId > pb->m_uniqueId)
+						b3Swap(pa, pb);
+#endif
+					m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), 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 / (b3Scalar)m_updates_call;
+	}
+	else
+	{
+		m_updates_ratio = 0;
+	}
+	m_updates_done /= 2;
+	m_updates_call /= 2;
+}
+
+//
+void b3DynamicBvhBroadphase::optimize()
+{
+	m_sets[0].optimizeTopDown();
+	m_sets[1].optimizeTopDown();
+}
+
+//
+b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
+{
+	return (m_paircache);
+}
+
+//
+const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
+{
+	return (m_paircache);
+}
+
+//
+void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
+{
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	bounds;
+
+	if (!m_sets[0].empty())
+		if (!m_sets[1].empty())
+			b3Merge(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 = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
+	aabbMin = bounds.Mins();
+	aabbMax = bounds.Maxs();
+}
+
+void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* 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_pid = 0;
+		m_cid = 0;
+		for (int i = 0; i <= STAGECOUNT; ++i)
+		{
+			m_stageRoots[i] = 0;
+		}
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::printStats()
+{
+}
+
+//
+#if B3_DBVT_BP_ENABLE_BENCHMARK
+
+struct b3BroadphaseBenchmark
+{
+	struct Experiment
+	{
+		const char* name;
+		int object_count;
+		int update_count;
+		int spawn_count;
+		int iterations;
+		b3Scalar speed;
+		b3Scalar amplitude;
+	};
+	struct Object
+	{
+		b3Vector3 center;
+		b3Vector3 extents;
+		b3BroadphaseProxy* proxy;
+		b3Scalar time;
+		void update(b3Scalar speed, b3Scalar amplitude, b3BroadphaseInterface* pbi)
+		{
+			time += speed;
+			center[0] = b3Cos(time * (b3Scalar)2.17) * amplitude +
+						b3Sin(time) * amplitude / 2;
+			center[1] = b3Cos(time * (b3Scalar)1.38) * amplitude +
+						b3Sin(time) * amplitude;
+			center[2] = b3Sin(time * (b3Scalar)0.777) * amplitude;
+			pbi->setAabb(proxy, center - extents, center + extents, 0);
+		}
+	};
+	static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
+	static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
+	static void OutputTime(const char* name, b3Clock& c, unsigned count = 0)
+	{
+		const unsigned long us = c.getTimeMicroseconds();
+		const unsigned long ms = (us + 500) / 1000;
+		const b3Scalar sec = us / (b3Scalar)(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 b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
+{
+	static const b3BroadphaseBenchmark::Experiment experiments[] =
+		{
+			{"1024o.10%", 1024, 10, 0, 8192, (b3Scalar)0.005, (b3Scalar)100},
+			/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
+		{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
+		};
+	static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
+	b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
+	b3Clock wallclock;
+	/* Begin			*/
+	for (int iexp = 0; iexp < nexperiments; ++iexp)
+	{
+		const b3BroadphaseBenchmark::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 b3Scalar speed = experiment.speed;
+		const b3Scalar 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)
+		{
+			b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object();
+			po->center[0] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->center[1] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->center[2] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->extents[0] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[1] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[2] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->time = b3BroadphaseBenchmark::UnitRand() * 2000;
+			po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
+			objects.push_back(po);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
+		/* First update		*/
+		wallclock.reset();
+		for (int i = 0; i < objects.size(); ++i)
+		{
+			objects[i]->update(speed, amplitude, pbi);
+		}
+		b3BroadphaseBenchmark::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);
+		}
+		b3BroadphaseBenchmark::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);
+		b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
+	}
+}
+#else
+/*void							b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
+{}
+*/
+#endif
+
+#if B3_DBVT_BP_PROFILE
+#undef b3SPC
+#endif

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h

@@ -0,0 +1,197 @@
+/*
+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.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+#ifndef B3_DBVT_BROADPHASE_H
+#define B3_DBVT_BROADPHASE_H
+
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include "b3BroadphaseCallback.h"
+
+//
+// Compile time config
+//
+
+#define B3_DBVT_BP_PROFILE 0
+//#define B3_DBVT_BP_SORTPAIRS				1
+#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
+#define B3_DBVT_BP_ACCURATESLEEPING 0
+#define B3_DBVT_BP_ENABLE_BENCHMARK 0
+#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
+
+#if B3_DBVT_BP_PROFILE
+#define B3_DBVT_BP_PROFILING_RATE 256
+
+#endif
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3BroadphaseProxy
+{
+	B3_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 b3CollisionObject or Rigidbody class
+	void* m_clientObject;
+	int m_collisionFilterGroup;
+	int m_collisionFilterMask;
+	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	b3Vector3 m_aabbMin;
+	b3Vector3 m_aabbMax;
+
+	B3_FORCE_INLINE int getUid() const
+	{
+		return m_uniqueId;
+	}
+
+	//used for memory pools
+	b3BroadphaseProxy() : m_clientObject(0)
+	{
+	}
+
+	b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+		: m_clientObject(userPtr),
+		  m_collisionFilterGroup(collisionFilterGroup),
+		  m_collisionFilterMask(collisionFilterMask),
+		  m_aabbMin(aabbMin),
+		  m_aabbMax(aabbMax)
+	{
+	}
+};
+
+//
+// b3DbvtProxy
+//
+struct b3DbvtProxy : b3BroadphaseProxy
+{
+	/* Fields		*/
+	//b3DbvtAabbMm	aabb;
+	b3DbvtNode* leaf;
+	b3DbvtProxy* links[2];
+	int stage;
+	/* ctor			*/
+
+	explicit b3DbvtProxy() {}
+	b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
+	{
+		links[0] = links[1] = 0;
+	}
+};
+
+typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
+
+///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
+///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 b3AxisSweep3 and b332BitAxisSweep3.
+struct b3DynamicBvhBroadphase
+{
+	/* Config		*/
+	enum
+	{
+		DYNAMIC_SET = 0, /* Dynamic set index	*/
+		FIXED_SET = 1,   /* Fixed set index		*/
+		STAGECOUNT = 2   /* Number of stages		*/
+	};
+	/* Fields		*/
+	b3DynamicBvh m_sets[2];                     // Dbvt sets
+	b3DbvtProxy* m_stageRoots[STAGECOUNT + 1];  // Stages list
+
+	b3AlignedObjectArray<b3DbvtProxy> m_proxies;
+	b3OverlappingPairCache* m_paircache;  // Pair cache
+	b3Scalar 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
+	b3Scalar m_updates_ratio;             // m_updates_done/m_updates_call
+	int m_pid;                            // Parse id
+	int m_cid;                            // Cleanup index
+	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 B3_DBVT_BP_PROFILE
+	b3Clock 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		*/
+	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
+	virtual ~b3DynamicBvhBroadphase();
+	void collide(b3Dispatcher* dispatcher);
+	void optimize();
+
+	/* b3BroadphaseInterface Implementation	*/
+	b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
+	virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
+	virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
+	virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
+
+	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
+	virtual b3OverlappingPairCache* getOverlappingPairCache();
+	virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
+	virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void printStats();
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(b3Dispatcher* dispatcher);
+
+	void performDeferredRemoval(b3Dispatcher* dispatcher);
+
+	void setVelocityPrediction(b3Scalar prediction)
+	{
+		m_prediction = prediction;
+	}
+	b3Scalar getVelocityPrediction() const
+	{
+		return m_prediction;
+	}
+
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
+	///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
+	///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(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
+
+	//static void						benchmark(b3BroadphaseInterface*);
+};
+
+#endif

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h

@@ -0,0 +1,70 @@
+/*
+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 B3_OVERLAPPING_PAIR_H
+#define B3_OVERLAPPING_PAIR_H
+
+#include "Bullet3Common/shared/b3Int4.h"
+
+#define B3_NEW_PAIR_MARKER -1
+#define B3_REMOVED_PAIR_MARKER -2
+
+typedef b3Int4 b3BroadphasePair;
+
+inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
+{
+	b3Int4 pair;
+
+	if (xx < yy)
+	{
+		pair.x = xx;
+		pair.y = yy;
+	}
+	else
+	{
+		pair.x = yy;
+		pair.y = xx;
+	}
+	pair.z = B3_NEW_PAIR_MARKER;
+	pair.w = B3_NEW_PAIR_MARKER;
+	return pair;
+}
+
+/*struct b3BroadphasePair : public b3Int4
+{
+	explicit b3BroadphasePair(){}
+	
+};
+*/
+
+class b3BroadphasePairSortPredicate
+{
+public:
+	bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
+	{
+		const int uidA0 = a.x;
+		const int uidB0 = b.x;
+		const int uidA1 = a.y;
+		const int uidB1 = b.y;
+		return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+	}
+};
+
+B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
+{
+	return (a.x == b.x) && (a.y == b.y);
+}
+
+#endif  //B3_OVERLAPPING_PAIR_H

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

@@ -0,0 +1,559 @@
+/*
+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 "b3OverlappingPairCache.h"
+
+//#include "b3Dispatcher.h"
+//#include "b3CollisionAlgorithm.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+
+#include <stdio.h>
+
+int b3g_overlappingPairs = 0;
+int b3g_removePairs = 0;
+int b3g_addedPairs = 0;
+int b3g_findPairs = 0;
+
+b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
+//,	m_blockedForChanges(false)
+{
+	int initialAllocatedSize = 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
+{
+}
+
+void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
+{
+	/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+		}
+	}
+	*/
+}
+
+void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
+{
+	class CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache* m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
+			}
+			return false;
+		}
+	};
+
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
+}
+
+void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
+{
+	class RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+					(pair.y == m_obsoleteProxy));
+		}
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback, dispatcher);
+}
+
+b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
+{
+	b3g_findPairs++;
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(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 != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == B3_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	b3Assert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void b3HashedOverlappingPairCache::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] = B3_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = B3_NULL_PAIR;
+		}
+
+		for (i = 0; i < curHashtableSize; i++)
+		{
+			const b3BroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.x;
+			int proxyId2 = pair.y;
+			/*if (proxyId1 > proxyId2) 
+				b3Swap(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;
+		}
+	}
+}
+
+b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
+{
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(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
+
+	b3BroadphasePair* 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();
+	pair = &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 = b3MakeBroadphasePair(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* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
+{
+	b3g_removePairs++;
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
+
+	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	cleanOverlappingPair(*pair, dispatcher);
+
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
+	b3Assert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int index = m_hashTable[hash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	int previous = B3_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(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 0;
+	}
+
+	// Remove the last pair from the hash table.
+	const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+	/* missing swap here too, Nat. */
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
+
+	index = m_hashTable[lastHash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	previous = B3_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(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 0;
+}
+//#include <stdio.h>
+
+void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
+{
+	int i;
+
+	//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i = 0; i < m_overlappingPairArray.size();)
+	{
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->x, pair->y, dispatcher);
+
+			b3g_overlappingPairs--;
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
+void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	///need to keep hashmap in sync with pair address, so rebuild all
+	b3BroadphasePairArray 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].x, tmpPairs[i].y, dispatcher);
+	}
+
+	for (i = 0; i < m_next.size(); i++)
+	{
+		m_next[i] = B3_NULL_PAIR;
+	}
+
+	tmpPairs.quickSort(b3BroadphasePairSortPredicate());
+
+	for (i = 0; i < tmpPairs.size(); i++)
+	{
+		addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
+	}
+}
+
+void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
+{
+	if (!hasDeferredRemoval())
+	{
+		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
+
+		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+		if (findIndex < m_overlappingPairArray.size())
+		{
+			b3g_overlappingPairs--;
+			b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
+
+			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 0;
+		}
+	}
+
+	return 0;
+}
+
+b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
+{
+	//don't add overlap with own
+	b3Assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0, proxy1))
+		return 0;
+
+	b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
+	*pair = b3MakeBroadphasePair(proxy0, proxy1);
+
+	b3g_overlappingPairs++;
+	b3g_addedPairs++;
+
+	//	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
+b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0, proxy1))
+		return 0;
+
+	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//b3Assert(it != m_overlappingPairSet.end());
+		b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+//#include <stdio.h>
+
+void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
+{
+	int i;
+
+	for (i = 0; i < m_overlappingPairArray.size();)
+	{
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			cleanOverlappingPair(*pair, dispatcher);
+			pair->x = -1;
+			pair->y = -1;
+			m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
+			m_overlappingPairArray.pop_back();
+			b3g_overlappingPairs--;
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
+b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
+															   m_hasDeferredRemoval(true),
+															   m_overlapFilterCallback(0)
+
+{
+	int initialAllocatedSize = 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
+{
+}
+
+void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
+{
+	/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+			b3g_removePairs--;
+		}
+	}
+	*/
+}
+
+void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
+{
+	class CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache* m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
+			}
+			return false;
+		}
+	};
+
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
+}
+
+void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
+{
+	class RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+					(pair.y == m_obsoleteProxy));
+		}
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback, dispatcher);
+}
+
+void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	//should already be sorted
+}

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h

@@ -0,0 +1,427 @@
+/*
+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 B3_OVERLAPPING_PAIR_CACHE_H
+#define B3_OVERLAPPING_PAIR_CACHE_H
+
+#include "Bullet3Common/shared/b3Int2.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+class b3Dispatcher;
+#include "b3OverlappingPair.h"
+
+typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
+
+struct b3OverlapCallback
+{
+	virtual ~b3OverlapCallback()
+	{
+	}
+	//return true for deletion of the pair
+	virtual bool processOverlap(b3BroadphasePair& pair) = 0;
+};
+
+struct b3OverlapFilterCallback
+{
+	virtual ~b3OverlapFilterCallback()
+	{
+	}
+	// return true when pairs need collision
+	virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
+};
+
+extern int b3g_removePairs;
+extern int b3g_addedPairs;
+extern int b3g_findPairs;
+
+const int B3_NULL_PAIR = 0xffffffff;
+
+///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
+///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
+class b3OverlappingPairCache
+{
+public:
+	virtual ~b3OverlappingPairCache() {}  // this is needed so we can get to the derived class destructor
+
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
+
+	virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
+
+	virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
+
+	virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
+
+	virtual int getNumOverlappingPairs() const = 0;
+
+	virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
+
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
+
+	virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
+
+	virtual bool hasDeferredRemoval() = 0;
+
+	//virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
+
+	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
+};
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+class b3HashedOverlappingPairCache : public b3OverlappingPairCache
+{
+	b3BroadphasePairArray m_overlappingPairArray;
+	b3OverlapFilterCallback* m_overlapFilterCallback;
+	//	bool		m_blockedForChanges;
+
+public:
+	b3HashedOverlappingPairCache();
+	virtual ~b3HashedOverlappingPairCache();
+
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
+
+	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
+
+		bool collides = true;  //(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 b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
+	{
+		b3g_addedPairs++;
+
+		if (!needsBroadphaseCollision(proxy0, proxy1))
+			return 0;
+
+		return internalAddPair(proxy0, proxy1);
+	}
+
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
+
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const b3BroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
+
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+	//	b3BroadphasePair* GetPairs() { return m_pairs; }
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+
+private:
+	b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
+
+	void growTables();
+
+	B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
+	{
+		return pair.x == proxyId1 && pair.y == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	B3_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;
+	}
+	*/
+
+	B3_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);
+	}
+
+	B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
+	{
+		int proxyId1 = proxy0;
+		int proxyId2 = proxy1;
+#if 0  // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+		if (proxyId1 > proxyId2) 
+			b3Swap(proxyId1, proxyId2);
+#endif
+
+		int index = m_hashTable[hash];
+
+		while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if (index == B3_NULL_PAIR)
+		{
+			return NULL;
+		}
+
+		b3Assert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return false;
+	}
+
+	/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+	{
+		m_ghostPairCallback = ghostPairCallback;
+	}
+	*/
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
+
+protected:
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+	//	b3OverlappingPairCallback*	m_ghostPairCallback;
+};
+
+///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
+class b3SortedOverlappingPairCache : public b3OverlappingPairCache
+{
+protected:
+	//avoid brute-force finding all the time
+	b3BroadphasePairArray 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
+	b3OverlapFilterCallback* m_overlapFilterCallback;
+
+	//		b3OverlappingPairCallback*	m_ghostPairCallback;
+
+public:
+	b3SortedOverlappingPairCache();
+	virtual ~b3SortedOverlappingPairCache();
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
+
+	void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
+
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
+
+	b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
+
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
+
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
+
+	inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
+
+		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+
+		return collides;
+	}
+
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const b3BroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return m_hasDeferredRemoval;
+	}
+
+	/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+		{
+			m_ghostPairCallback = ghostPairCallback;
+		}
+		*/
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
+};
+
+///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
+class b3NullPairCache : public b3OverlappingPairCache
+{
+	b3BroadphasePairArray m_overlappingPairArray;
+
+public:
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual int getNumOverlappingPairs() const
+	{
+		return 0;
+	}
+
+	virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
+	{
+	}
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return true;
+	}
+
+	//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
+	//	{
+	//
+	//	}
+
+	virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
+	{
+		return 0;
+	}
+
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
+	{
+		(void)dispatcher;
+	}
+};
+
+#endif  //B3_OVERLAPPING_PAIR_CACHE_H

Engine/lib/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h

@@ -0,0 +1,56 @@
+
+#ifndef B3_AABB_H
+#define B3_AABB_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+typedef struct b3Aabb b3Aabb_t;
+
+struct b3Aabb
+{
+	union {
+		float m_min[4];
+		b3Float4 m_minVec;
+		int m_minIndices[4];
+	};
+	union {
+		float m_max[4];
+		b3Float4 m_maxVec;
+		int m_signedMaxIndices[4];
+	};
+};
+
+inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
+							 b3Float4ConstArg pos,
+							 b3QuatConstArg orn,
+							 b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
+{
+	b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
+	localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
+	b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
+	b3Mat3x3 m;
+	m = b3QuatGetRotationMatrix(orn);
+	b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
+	b3Float4 center = b3TransformPoint(localCenter, pos, orn);
+
+	b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
+								   0.f);
+	*aabbMinOut = center - extent;
+	*aabbMaxOut = center + extent;
+}
+
+/// conservative test for overlap between two aabbs
+inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
+								  b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
+	overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;
+	overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;
+	return overlap;
+}
+
+#endif  //B3_AABB_H

Engine/lib/bullet/src/Bullet3Collision/CMakeLists.txt

@@ -0,0 +1,93 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Collision_SRCS
+	BroadPhaseCollision/b3DynamicBvh.cpp
+	BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
+	BroadPhaseCollision/b3OverlappingPairCache.cpp
+	NarrowPhaseCollision/b3ConvexUtility.cpp
+	NarrowPhaseCollision/b3CpuNarrowPhase.cpp
+)
+
+SET(Bullet3CollisionBroadPhase_HDRS
+	BroadPhaseCollision/b3BroadphaseCallback.h
+	BroadPhaseCollision/b3DynamicBvh.h
+	BroadPhaseCollision/b3DynamicBvhBroadphase.h
+	BroadPhaseCollision/b3OverlappingPair.h
+	BroadPhaseCollision/b3OverlappingPairCache.h
+)
+SET(Bullet3CollisionBroadPhaseShared_HDRS
+	BroadPhaseCollision/shared/b3Aabb.h
+)
+
+SET(Bullet3CollisionNarrowPhase_HDRS
+	NarrowPhaseCollision/b3Config.h
+	NarrowPhaseCollision/b3Contact4.h
+	NarrowPhaseCollision/b3ConvexUtility.h
+	NarrowPhaseCollision/b3CpuNarrowPhase.h
+	NarrowPhaseCollision/b3RaycastInfo.h
+	NarrowPhaseCollision/b3RigidBodyCL.h
+)
+SET(Bullet3CollisionNarrowPhaseShared_HDRS
+
+	NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
+	NarrowPhaseCollision/shared/b3BvhTraversal.h
+	NarrowPhaseCollision/shared/b3ClipFaces.h
+	NarrowPhaseCollision/shared/b3Collidable.h
+	NarrowPhaseCollision/shared/b3Contact4Data.h
+	NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
+	NarrowPhaseCollision/shared/b3ContactSphereSphere.h
+	NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
+	NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
+	NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
+	NarrowPhaseCollision/shared/b3MprPenetration.h
+	NarrowPhaseCollision/shared/b3NewContactReduction.h
+	NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
+	NarrowPhaseCollision/shared/b3ReduceContacts.h
+	NarrowPhaseCollision/shared/b3RigidBodyData.h
+	NarrowPhaseCollision/shared/b3UpdateAabbs.h
+)
+
+SET(Bullet3Collision_HDRS
+	${Bullet3CollisionBroadPhase_HDRS}
+	${Bullet3CollisionBroadPhaseShared_HDRS}
+	${Bullet3CollisionNarrowPhaseShared_HDRS}
+	${Bullet3CollisionNarrowPhase_HDRS}
+)
+
+ADD_LIBRARY(Bullet3Collision ${Bullet3Collision_SRCS} ${Bullet3Collision_HDRS})
+if (BUILD_SHARED_LIBS)
+  target_link_libraries(Bullet3Collision Bullet3Geometry)
+endif ()
+SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Collision DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Collision
+					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)
+			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(Bullet3Collision PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES PUBLIC_HEADER "${Bullet3Collision_HDRS}")
+			# Have to list out sub-directories manually:
+			#todo
+			#SET_PROPERTY(SOURCE ${Bullet3CollisionBroadPhase_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadPhaseCollision)
+
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h

@@ -0,0 +1,39 @@
+#ifndef B3_CONFIG_H
+#define B3_CONFIG_H
+
+struct b3Config
+{
+	int m_maxConvexBodies;
+	int m_maxConvexShapes;
+	int m_maxBroadphasePairs;
+	int m_maxContactCapacity;
+	int m_compoundPairCapacity;
+
+	int m_maxVerticesPerFace;
+	int m_maxFacesPerShape;
+	int m_maxConvexVertices;
+	int m_maxConvexIndices;
+	int m_maxConvexUniqueEdges;
+
+	int m_maxCompoundChildShapes;
+
+	int m_maxTriConvexPairCapacity;
+
+	b3Config()
+		: m_maxConvexBodies(128 * 1024),
+		  m_maxVerticesPerFace(64),
+		  m_maxFacesPerShape(12),
+		  m_maxConvexVertices(8192),
+		  m_maxConvexIndices(81920),
+		  m_maxConvexUniqueEdges(8192),
+		  m_maxCompoundChildShapes(8192),
+		  m_maxTriConvexPairCapacity(256 * 1024)
+	{
+		m_maxConvexShapes = m_maxConvexBodies;
+		m_maxBroadphasePairs = 16 * m_maxConvexBodies;
+		m_maxContactCapacity = m_maxBroadphasePairs;
+		m_compoundPairCapacity = 1024 * 1024;
+	}
+};
+
+#endif  //B3_CONFIG_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -0,0 +1,55 @@
+/*
+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 B3_CONTACT4_H
+#define B3_CONTACT4_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3Contact4 : public b3Contact4Data
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
+	int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
+	bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
+	bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
+	//	todo. make it safer
+	int& getBatchIdx() { return m_batchIdx; }
+	const int& getBatchIdx() const { return m_batchIdx; }
+	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
+	void setRestituitionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
+	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
+	void setFrictionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
+
+	//float& getNPoints() { return m_worldNormal[3]; }
+	int getNPoints() const { return (int)m_worldNormalOnB.w; }
+
+	float getPenetration(int idx) const { return m_worldPosB[idx].w; }
+
+	bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
+};
+
+#endif  //B3_CONTACT4_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

@@ -0,0 +1,500 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+#include "b3ConvexUtility.h"
+#include "Bullet3Geometry/b3ConvexHullComputer.h"
+#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3HashMap.h"
+
+b3ConvexUtility::~b3ConvexUtility()
+{
+}
+
+bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
+{
+	b3ConvexHullComputer conv;
+	conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
+
+	b3AlignedObjectArray<b3Vector3> faceNormals;
+	int numFaces = conv.faces.size();
+	faceNormals.resize(numFaces);
+	b3ConvexHullComputer* convexUtil = &conv;
+
+	b3AlignedObjectArray<b3MyFace> tmpFaces;
+	tmpFaces.resize(numFaces);
+
+	int numVertices = convexUtil->vertices.size();
+	m_vertices.resize(numVertices);
+	for (int p = 0; p < numVertices; p++)
+	{
+		m_vertices[p] = convexUtil->vertices[p];
+	}
+
+	for (int i = 0; i < numFaces; i++)
+	{
+		int face = convexUtil->faces[i];
+		//printf("face=%d\n",face);
+		const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
+		const b3ConvexHullComputer::Edge* edge = firstEdge;
+
+		b3Vector3 edges[3];
+		int numEdges = 0;
+		//compute face normals
+
+		do
+		{
+			int src = edge->getSourceVertex();
+			tmpFaces[i].m_indices.push_back(src);
+			int targ = edge->getTargetVertex();
+			b3Vector3 wa = convexUtil->vertices[src];
+
+			b3Vector3 wb = convexUtil->vertices[targ];
+			b3Vector3 newEdge = wb - wa;
+			newEdge.normalize();
+			if (numEdges < 2)
+				edges[numEdges++] = newEdge;
+
+			edge = edge->getNextEdgeOfFace();
+		} while (edge != firstEdge);
+
+		b3Scalar planeEq = 1e30f;
+
+		if (numEdges == 2)
+		{
+			faceNormals[i] = edges[0].cross(edges[1]);
+			faceNormals[i].normalize();
+			tmpFaces[i].m_plane[0] = faceNormals[i].getX();
+			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
+			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
+			tmpFaces[i].m_plane[3] = planeEq;
+		}
+		else
+		{
+			b3Assert(0);  //degenerate?
+			faceNormals[i].setZero();
+		}
+
+		for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
+		{
+			b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
+			if (planeEq > eq)
+			{
+				planeEq = eq;
+			}
+		}
+		tmpFaces[i].m_plane[3] = -planeEq;
+	}
+
+	//merge coplanar faces and copy them to m_polyhedron
+
+	b3Scalar faceWeldThreshold = 0.999f;
+	b3AlignedObjectArray<int> todoFaces;
+	for (int i = 0; i < tmpFaces.size(); i++)
+		todoFaces.push_back(i);
+
+	while (todoFaces.size())
+	{
+		b3AlignedObjectArray<int> coplanarFaceGroup;
+		int refFace = todoFaces[todoFaces.size() - 1];
+
+		coplanarFaceGroup.push_back(refFace);
+		b3MyFace& faceA = tmpFaces[refFace];
+		todoFaces.pop_back();
+
+		b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
+		for (int j = todoFaces.size() - 1; j >= 0; j--)
+		{
+			int i = todoFaces[j];
+			b3MyFace& faceB = tmpFaces[i];
+			b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
+			if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
+			{
+				coplanarFaceGroup.push_back(i);
+				todoFaces.remove(i);
+			}
+		}
+
+		bool did_merge = false;
+		if (coplanarFaceGroup.size() > 1)
+		{
+			//do the merge: use Graham Scan 2d convex hull
+
+			b3AlignedObjectArray<b3GrahamVector3> orgpoints;
+			b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
+
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
+			{
+				//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+
+				b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
+				b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
+				averageFaceNormal += faceNormal;
+				for (int f = 0; f < face.m_indices.size(); f++)
+				{
+					int orgIndex = face.m_indices[f];
+					b3Vector3 pt = m_vertices[orgIndex];
+
+					bool found = false;
+
+					for (int i = 0; i < orgpoints.size(); i++)
+					{
+						//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
+						if (orgpoints[i].m_orgIndex == orgIndex)
+						{
+							found = true;
+							break;
+						}
+					}
+					if (!found)
+						orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
+				}
+			}
+
+			b3MyFace combinedFace;
+			for (int i = 0; i < 4; i++)
+				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
+
+			b3AlignedObjectArray<b3GrahamVector3> hull;
+
+			averageFaceNormal.normalize();
+			b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
+
+			for (int i = 0; i < hull.size(); i++)
+			{
+				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
+				for (int k = 0; k < orgpoints.size(); k++)
+				{
+					if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
+					{
+						orgpoints[k].m_orgIndex = -1;  // invalidate...
+						break;
+					}
+				}
+			}
+
+			// are there rejected vertices?
+			bool reject_merge = false;
+
+			for (int i = 0; i < orgpoints.size(); i++)
+			{
+				if (orgpoints[i].m_orgIndex == -1)
+					continue;  // this is in the hull...
+				// this vertex is rejected -- is anybody else using this vertex?
+				for (int j = 0; j < tmpFaces.size(); j++)
+				{
+					b3MyFace& face = tmpFaces[j];
+					// is this a face of the current coplanar group?
+					bool is_in_current_group = false;
+					for (int k = 0; k < coplanarFaceGroup.size(); k++)
+					{
+						if (coplanarFaceGroup[k] == j)
+						{
+							is_in_current_group = true;
+							break;
+						}
+					}
+					if (is_in_current_group)  // ignore this face...
+						continue;
+					// does this face use this rejected vertex?
+					for (int v = 0; v < face.m_indices.size(); v++)
+					{
+						if (face.m_indices[v] == orgpoints[i].m_orgIndex)
+						{
+							// this rejected vertex is used in another face -- reject merge
+							reject_merge = true;
+							break;
+						}
+					}
+					if (reject_merge)
+						break;
+				}
+				if (reject_merge)
+					break;
+			}
+
+			if (!reject_merge)
+			{
+				// do this merge!
+				did_merge = true;
+				m_faces.push_back(combinedFace);
+			}
+		}
+		if (!did_merge)
+		{
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
+			{
+				b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
+				m_faces.push_back(face);
+			}
+		}
+	}
+
+	initialize();
+
+	return true;
+}
+
+inline bool IsAlmostZero(const b3Vector3& v)
+{
+	if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
+	return true;
+}
+
+struct b3InternalVertexPair
+{
+	b3InternalVertexPair(short int v0, short int v1)
+		: m_v0(v0),
+		  m_v1(v1)
+	{
+		if (m_v1 > m_v0)
+			b3Swap(m_v0, m_v1);
+	}
+	short int m_v0;
+	short int m_v1;
+	int getHash() const
+	{
+		return m_v0 + (m_v1 << 16);
+	}
+	bool equals(const b3InternalVertexPair& other) const
+	{
+		return m_v0 == other.m_v0 && m_v1 == other.m_v1;
+	}
+};
+
+struct b3InternalEdge
+{
+	b3InternalEdge()
+		: m_face0(-1),
+		  m_face1(-1)
+	{
+	}
+	short int m_face0;
+	short int m_face1;
+};
+
+//
+
+#ifdef TEST_INTERNAL_OBJECTS
+bool b3ConvexUtility::testContainment() const
+{
+	for (int p = 0; p < 8; p++)
+	{
+		b3Vector3 LocalPt;
+		if (p == 0)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 1)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 2)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 3)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
+		else if (p == 4)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 5)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 6)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 7)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
+
+		for (int i = 0; i < m_faces.size(); i++)
+		{
+			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
+			const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
+			if (d > 0.0f)
+				return false;
+		}
+	}
+	return true;
+}
+#endif
+
+void b3ConvexUtility::initialize()
+{
+	b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
+
+	b3Scalar TotalArea = 0.0f;
+
+	m_localCenter.setValue(0, 0, 0);
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		int NbTris = numVertices;
+		for (int j = 0; j < NbTris; j++)
+		{
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
+			b3InternalEdge* edptr = edges.find(vp);
+			b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
+			edge.normalize();
+
+			bool found = false;
+			b3Vector3 diff, diff2;
+
+			for (int p = 0; p < m_uniqueEdges.size(); p++)
+			{
+				diff = m_uniqueEdges[p] - edge;
+				diff2 = m_uniqueEdges[p] + edge;
+
+				//	if ((diff.length2()==0.f) ||
+				//	(diff2.length2()==0.f))
+
+				if (IsAlmostZero(diff) ||
+					IsAlmostZero(diff2))
+				{
+					found = true;
+					break;
+				}
+			}
+
+			if (!found)
+			{
+				m_uniqueEdges.push_back(edge);
+			}
+
+			if (edptr)
+			{
+				//TBD: figure out why I added this assert
+				//				b3Assert(edptr->m_face0>=0);
+				//			b3Assert(edptr->m_face1<0);
+				edptr->m_face1 = i;
+			}
+			else
+			{
+				b3InternalEdge ed;
+				ed.m_face0 = i;
+				edges.insert(vp, ed);
+			}
+		}
+	}
+
+#ifdef USE_CONNECTED_FACES
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		m_faces[i].m_connectedFaces.resize(numVertices);
+
+		for (int j = 0; j < numVertices; j++)
+		{
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
+			b3InternalEdge* edptr = edges.find(vp);
+			b3Assert(edptr);
+			b3Assert(edptr->m_face0 >= 0);
+			b3Assert(edptr->m_face1 >= 0);
+
+			int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
+			m_faces[i].m_connectedFaces[j] = connectedFace;
+		}
+	}
+#endif  //USE_CONNECTED_FACES
+
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		int NbTris = numVertices - 2;
+
+		const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
+		for (int j = 1; j <= NbTris; j++)
+		{
+			int k = (j + 1) % numVertices;
+			const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
+			const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
+			b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
+			b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
+			m_localCenter += Area * Center;
+			TotalArea += Area;
+		}
+	}
+	m_localCenter /= TotalArea;
+
+#ifdef TEST_INTERNAL_OBJECTS
+	if (1)
+	{
+		m_radius = FLT_MAX;
+		for (int i = 0; i < m_faces.size(); i++)
+		{
+			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
+			const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
+			if (dist < m_radius)
+				m_radius = dist;
+		}
+
+		b3Scalar MinX = FLT_MAX;
+		b3Scalar MinY = FLT_MAX;
+		b3Scalar MinZ = FLT_MAX;
+		b3Scalar MaxX = -FLT_MAX;
+		b3Scalar MaxY = -FLT_MAX;
+		b3Scalar MaxZ = -FLT_MAX;
+		for (int i = 0; i < m_vertices.size(); i++)
+		{
+			const b3Vector3& pt = m_vertices[i];
+			if (pt.getX() < MinX) MinX = pt.getX();
+			if (pt.getX() > MaxX) MaxX = pt.getX();
+			if (pt.getY() < MinY) MinY = pt.getY();
+			if (pt.getY() > MaxY) MaxY = pt.getY();
+			if (pt.getZ() < MinZ) MinZ = pt.getZ();
+			if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
+		}
+		mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
+		mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
+
+		//		const b3Scalar r = m_radius / sqrtf(2.0f);
+		const b3Scalar r = m_radius / sqrtf(3.0f);
+		const int LargestExtent = mE.maxAxis();
+		const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
+		m_extents[0] = m_extents[1] = m_extents[2] = r;
+		m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
+		bool FoundBox = false;
+		for (int j = 0; j < 1024; j++)
+		{
+			if (testContainment())
+			{
+				FoundBox = true;
+				break;
+			}
+
+			m_extents[LargestExtent] -= Step;
+		}
+		if (!FoundBox)
+		{
+			m_extents[0] = m_extents[1] = m_extents[2] = r;
+		}
+		else
+		{
+			// Refine the box
+			const b3Scalar Step = (m_radius - r) / 1024.0f;
+			const int e0 = (1 << LargestExtent) & 3;
+			const int e1 = (1 << e0) & 3;
+
+			for (int j = 0; j < 1024; j++)
+			{
+				const b3Scalar Saved0 = m_extents[e0];
+				const b3Scalar Saved1 = m_extents[e1];
+				m_extents[e0] += Step;
+				m_extents[e1] += Step;
+
+				if (!testContainment())
+				{
+					m_extents[e0] = Saved0;
+					m_extents[e1] = Saved1;
+					break;
+				}
+			}
+		}
+	}
+#endif
+}

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h

@@ -0,0 +1,55 @@
+
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+#ifndef _BT_CONVEX_UTILITY_H
+#define _BT_CONVEX_UTILITY_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Transform.h"
+
+struct b3MyFace
+{
+	b3AlignedObjectArray<int> m_indices;
+	b3Scalar m_plane[4];
+};
+
+B3_ATTRIBUTE_ALIGNED16(class)
+b3ConvexUtility
+{
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3 m_localCenter;
+	b3Vector3 m_extents;
+	b3Vector3 mC;
+	b3Vector3 mE;
+	b3Scalar m_radius;
+
+	b3AlignedObjectArray<b3Vector3> m_vertices;
+	b3AlignedObjectArray<b3MyFace> m_faces;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+
+	b3ConvexUtility()
+	{
+	}
+	virtual ~b3ConvexUtility();
+
+	bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
+
+	void initialize();
+	bool testContainment() const;
+};
+#endif

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

@@ -0,0 +1,297 @@
+#include "b3CpuNarrowPhase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
+
+struct b3CpuNarrowPhaseInternalData
+{
+	b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
+	b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
+	b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
+	b3Config m_config;
+
+	b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+	b3AlignedObjectArray<b3Vector3> m_convexVertices;
+	b3AlignedObjectArray<int> m_convexIndices;
+	b3AlignedObjectArray<b3GpuFace> m_convexFaces;
+
+	b3AlignedObjectArray<b3Contact4Data> m_contacts;
+
+	int m_numAcceleratedShapes;
+};
+
+const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
+{
+	return m_data->m_contacts;
+}
+
+b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
+{
+	m_data = new b3CpuNarrowPhaseInternalData;
+	m_data->m_config = config;
+	m_data->m_numAcceleratedShapes = 0;
+}
+
+b3CpuNarrowPhase::~b3CpuNarrowPhase()
+{
+	delete m_data;
+}
+
+void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
+{
+	int nPairs = pairs.size();
+	int numContacts = 0;
+	int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
+	m_data->m_contacts.resize(maxContactCapacity);
+
+	for (int i = 0; i < nPairs; i++)
+	{
+		int bodyIndexA = pairs[i].x;
+		int bodyIndexB = pairs[i].y;
+		int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
+		int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//			computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
+		{
+			//			computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//printf("convex-sphere\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
+		{
+			//			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			//			computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
+			//			nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
+		{
+			//			computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			//			computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//printf("pairs[i].z=%d\n",pairs[i].z);
+			//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
+			//		m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
+			int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
+														m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
+
+			if (contactIndex >= 0)
+			{
+				pairs[i].z = contactIndex;
+			}
+			//			printf("plane-convex\n");
+		}
+	}
+
+	m_data->m_contacts.resize(numContacts);
+}
+
+int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+{
+	int collidableIndex = allocateCollidable();
+	if (collidableIndex < 0)
+		return collidableIndex;
+
+	b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
+	col.m_shapeType = SHAPE_CONVEX_HULL;
+	col.m_shapeIndex = -1;
+
+	{
+		b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+			localCenter += utilPtr->m_vertices[i];
+		localCenter *= (1.f / utilPtr->m_vertices.size());
+		utilPtr->m_localCenter = localCenter;
+
+		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
+	}
+
+	if (col.m_shapeIndex >= 0)
+	{
+		b3Aabb aabb;
+
+		b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
+		b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
+
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+		{
+			myAabbMin.setMin(utilPtr->m_vertices[i]);
+			myAabbMax.setMax(utilPtr->m_vertices[i]);
+		}
+		aabb.m_min[0] = myAabbMin[0];
+		aabb.m_min[1] = myAabbMin[1];
+		aabb.m_min[2] = myAabbMin[2];
+		aabb.m_minIndices[3] = 0;
+
+		aabb.m_max[0] = myAabbMax[0];
+		aabb.m_max[1] = myAabbMax[1];
+		aabb.m_max[2] = myAabbMax[2];
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU.push_back(aabb);
+	}
+
+	return collidableIndex;
+}
+
+int b3CpuNarrowPhase::allocateCollidable()
+{
+	int curSize = m_data->m_collidablesCPU.size();
+	if (curSize < m_data->m_config.m_maxConvexShapes)
+	{
+		m_data->m_collidablesCPU.expand();
+		return curSize;
+	}
+	else
+	{
+		b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
+	}
+	return -1;
+}
+
+int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+{
+	b3AlignedObjectArray<b3Vector3> verts;
+
+	unsigned char* vts = (unsigned char*)vertices;
+	for (int i = 0; i < numVertices; i++)
+	{
+		float* vertex = (float*)&vts[i * strideInBytes];
+		verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
+	}
+
+	b3ConvexUtility* utilPtr = new b3ConvexUtility();
+	bool merge = true;
+	if (numVertices)
+	{
+		utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
+	}
+
+	int collidableIndex = registerConvexHullShape(utilPtr);
+
+	delete utilPtr;
+	return collidableIndex;
+}
+
+int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
+{
+	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
+
+	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
+	convex.mC = convexPtr->mC;
+	convex.mE = convexPtr->mE;
+	convex.m_extents = convexPtr->m_extents;
+	convex.m_localCenter = convexPtr->m_localCenter;
+	convex.m_radius = convexPtr->m_radius;
+
+	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
+	int edgeOffset = m_data->m_uniqueEdges.size();
+	convex.m_uniqueEdgesOffset = edgeOffset;
+
+	m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
+
+	//convex data here
+	int i;
+	for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
+	{
+		m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
+	}
+
+	int faceOffset = m_data->m_convexFaces.size();
+	convex.m_faceOffset = faceOffset;
+	convex.m_numFaces = convexPtr->m_faces.size();
+
+	m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
+
+	for (i = 0; i < convexPtr->m_faces.size(); i++)
+	{
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
+																			   convexPtr->m_faces[i].m_plane[1],
+																			   convexPtr->m_faces[i].m_plane[2],
+																			   convexPtr->m_faces[i].m_plane[3]);
+
+		int indexOffset = m_data->m_convexIndices.size();
+		int numIndices = convexPtr->m_faces[i].m_indices.size();
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset + numIndices);
+		for (int p = 0; p < numIndices; p++)
+		{
+			m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
+		}
+	}
+
+	convex.m_numVertices = convexPtr->m_vertices.size();
+	int vertexOffset = m_data->m_convexVertices.size();
+	convex.m_vertexOffset = vertexOffset;
+
+	m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
+	for (int i = 0; i < convexPtr->m_vertices.size(); i++)
+	{
+		m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
+	}
+
+	(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
+
+	return m_data->m_numAcceleratedShapes++;
+}
+
+const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
+{
+	return m_data->m_localShapeAABBCPU[collidableIndex];
+}

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

@@ -0,0 +1,92 @@
+#ifndef B3_CPU_NARROWPHASE_H
+#define B3_CPU_NARROWPHASE_H
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+class b3CpuNarrowPhase
+{
+protected:
+	struct b3CpuNarrowPhaseInternalData* m_data;
+	int m_acceleratedCompanionShapeIndex;
+	int m_planeBodyIndex;
+	int m_static0Index;
+
+	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
+	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
+
+public:
+	b3CpuNarrowPhase(const struct b3Config& config);
+
+	virtual ~b3CpuNarrowPhase(void);
+
+	int registerSphereShape(float radius);
+	int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+
+	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
+	int registerFace(const b3Vector3& faceNormal, float faceConstant);
+
+	int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
+
+	//do they need to be merged?
+
+	int registerConvexHullShape(b3ConvexUtility* utilPtr);
+	int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+
+	//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
+	void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
+
+	void writeAllBodiesToGpu();
+	void reset();
+	void readbackAllBodiesToCpu();
+	bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
+
+	void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
+	void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
+
+	//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
+	virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
+
+	const struct b3RigidBodyData* getBodiesCpu() const;
+	//struct b3RigidBodyData* getBodiesCpu();
+
+	int getNumBodiesGpu() const;
+
+	int getNumBodyInertiasGpu() const;
+
+	const struct b3Collidable* getCollidablesCpu() const;
+	int getNumCollidablesGpu() const;
+
+	/*const struct b3Contact4* getContactsCPU() const;
+
+	
+	int	getNumContactsGpu() const;
+	*/
+
+	const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
+
+	int getNumRigidBodies() const;
+
+	int allocateCollidable();
+
+	int getStatic0Index() const
+	{
+		return m_static0Index;
+	}
+	b3Collidable& getCollidableCpu(int collidableIndex);
+	const b3Collidable& getCollidableCpu(int collidableIndex) const;
+
+	const b3CpuNarrowPhaseInternalData* getInternalData() const
+	{
+		return m_data;
+	}
+
+	const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
+};
+
+#endif  //B3_CPU_NARROWPHASE_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h

@@ -0,0 +1,25 @@
+
+#ifndef B3_RAYCAST_INFO_H
+#define B3_RAYCAST_INFO_H
+
+#include "Bullet3Common/b3Vector3.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayInfo
+{
+	b3Vector3 m_from;
+	b3Vector3 m_to;
+};
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayHit
+{
+	b3Scalar m_hitFraction;
+	int m_hitBody;
+	int m_hitResult1;
+	int m_hitResult2;
+	b3Vector3 m_hitPoint;
+	b3Vector3 m_hitNormal;
+};
+
+#endif  //B3_RAYCAST_INFO_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h

@@ -0,0 +1,28 @@
+/*
+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 B3_RIGID_BODY_CL
+#define B3_RIGID_BODY_CL
+
+#include "Bullet3Common/b3Scalar.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+inline float b3GetInvMass(const b3RigidBodyData& body)
+{
+	return body.m_invMass;
+}
+
+#endif  //B3_RIGID_BODY_CL

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h

@@ -0,0 +1,19 @@
+
+#ifndef B3_BVH_SUBTREE_INFO_DATA_H
+#define B3_BVH_SUBTREE_INFO_DATA_H
+
+typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
+
+struct b3BvhSubtreeInfoData
+{
+	//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];
+};
+
+#endif  //B3_BVH_SUBTREE_INFO_DATA_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

@@ -0,0 +1,123 @@
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+
+// work-in-progress
+void b3BvhTraversal(__global const b3Int4* pairs,
+					__global const b3RigidBodyData* rigidBodies,
+					__global const b3Collidable* collidables,
+					__global b3Aabb* aabbs,
+					__global b3Int4* concavePairsOut,
+					__global volatile int* numConcavePairsOut,
+					__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
+					__global const b3QuantizedBvhNode* quantizedNodesRoot,
+					__global const b3BvhInfo* bvhInfos,
+					int numPairs,
+					int maxNumConcavePairsCapacity,
+					int id)
+{
+	int bodyIndexA = pairs[id].x;
+	int bodyIndexB = pairs[id].y;
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	//once the broadphase avoids static-static pairs, we can remove this test
+	if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
+	{
+		return;
+	}
+
+	if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
+		return;
+
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+
+	if (shapeTypeB != SHAPE_CONVEX_HULL &&
+		shapeTypeB != SHAPE_SPHERE &&
+		shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		return;
+
+	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
+
+	b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
+	b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
+	b3Float4 bvhQuantization = bvhInfo.m_quantization;
+	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
+	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
+	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
+	b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
+
+	for (int i = 0; i < numSubtreeHeaders; i++)
+	{
+		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
+
+		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			int startNodeIndex = subtree.m_rootNodeIndex;
+			int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
+			int curIndex = startNodeIndex;
+			int escapeIndex;
+			int isLeafNode;
+			int aabbOverlap;
+			while (curIndex < endNodeIndex)
+			{
+				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
+				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
+				isLeafNode = b3IsLeaf(&rootNode);
+				if (aabbOverlap)
+				{
+					if (isLeafNode)
+					{
+						int triangleIndex = b3GetTriangleIndex(&rootNode);
+						if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+						{
+							int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
+							int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
+							for (int b = 0; b < numChildrenB; b++)
+							{
+								if ((pairIdx + b) < maxNumConcavePairsCapacity)
+								{
+									int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
+									b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
+									concavePairsOut[pairIdx + b] = newPair;
+								}
+							}
+						}
+						else
+						{
+							int pairIdx = b3AtomicInc(numConcavePairsOut);
+							if (pairIdx < maxNumConcavePairsCapacity)
+							{
+								b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
+								concavePairsOut[pairIdx] = newPair;
+							}
+						}
+					}
+					curIndex++;
+				}
+				else
+				{
+					if (isLeafNode)
+					{
+						curIndex++;
+					}
+					else
+					{
+						escapeIndex = b3GetEscapeIndex(&rootNode);
+						curIndex += escapeIndex;
+					}
+				}
+			}
+		}
+	}
+}

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

@@ -0,0 +1,171 @@
+#ifndef B3_CLIP_FACES_H
+#define B3_CLIP_FACES_H
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
+{
+	return b3MakeFloat4(a.x + (b.x - a.x) * t,
+						a.y + (b.y - a.y) * t,
+						a.z + (b.z - a.z) * t,
+						0.f);
+}
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
+{
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+	//double-check next test
+	//	if (numVertsIn < 2)
+	//		return 0;
+
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
+	b3Float4 endVertex = pVtxIn[0];
+
+	ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
+
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex = pVtxIn[ve];
+		de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
+		if (ds < 0)
+		{
+			if (de < 0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+			}
+		}
+		else
+		{
+			if (de < 0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
+											 __global const int* hasSeparatingAxis,
+											 __global b3Int4* clippingFacesOut,
+											 __global b3Float4* worldVertsA1,
+											 __global b3Float4* worldNormalsA1,
+											 __global b3Float4* worldVertsB1,
+											 __global b3Float4* worldVertsB2,
+											 int vertexFaceCapacity,
+											 int pairIndex)
+{
+	//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+
+	float minDist = -1e30f;
+	float maxDist = 0.02f;
+
+	//	if (i<numPairs)
+	{
+		if (hasSeparatingAxis[i])
+		{
+			//			int bodyIndexA = pairs[i].x;
+			//		int bodyIndexB = pairs[i].y;
+
+			int numLocalContactsOut = 0;
+
+			int capacityWorldVertsB2 = vertexFaceCapacity;
+
+			__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
+			__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
+
+			{
+				__global b3Int4* clippingFaces = clippingFacesOut;
+
+				int closestFaceA = clippingFaces[pairIndex].x;
+				// int closestFaceB = clippingFaces[pairIndex].y;
+				int numVertsInA = clippingFaces[pairIndex].z;
+				int numVertsInB = clippingFaces[pairIndex].w;
+
+				int numVertsOut = 0;
+
+				if (closestFaceA >= 0)
+				{
+					// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+
+					for (int e0 = 0; e0 < numVertsInA; e0++)
+					{
+						const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
+						const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
+						const b3Float4 WorldEdge0 = aw - bw;
+						b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
+						b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
+						b3Float4 worldA1 = aw;
+						float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
+						b3Float4 planeNormalWS = planeNormalWS1;
+						float planeEqWS = planeEqWS1;
+						numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
+						__global b3Float4* tmp = pVtxOut;
+						pVtxOut = pVtxIn;
+						pVtxIn = tmp;
+						numVertsInB = numVertsOut;
+						numVertsOut = 0;
+					}
+
+					b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
+					float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
+
+					for (int i = 0; i < numVertsInB; i++)
+					{
+						float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
+						if (depth <= minDist)
+						{
+							depth = minDist;
+						}
+						/*
+						static float maxDepth = 0.f;
+						if (depth < maxDepth)
+						{
+							maxDepth = depth;
+							if (maxDepth < -10)
+							{
+								printf("error at framecount %d?\n",myframecount);
+							}
+							printf("maxDepth = %f\n", maxDepth);
+
+						}
+*/
+						if (depth <= maxDist)
+						{
+							b3Float4 pointInWorld = pVtxIn[i];
+							pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+						}
+					}
+				}
+				clippingFaces[pairIndex].w = numLocalContactsOut;
+			}
+
+			for (int i = 0; i < numLocalContactsOut; i++)
+				pVtxIn[i] = pVtxOut[i];
+
+		}  //		if (hasSeparatingAxis[i])
+	}      //	if (i<numPairs)
+}
+
+#endif  //B3_CLIP_FACES_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h

@@ -0,0 +1,69 @@
+
+#ifndef B3_COLLIDABLE_H
+#define B3_COLLIDABLE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+enum b3ShapeTypes
+{
+	SHAPE_HEIGHT_FIELD = 1,
+
+	SHAPE_CONVEX_HULL = 3,
+	SHAPE_PLANE = 4,
+	SHAPE_CONCAVE_TRIMESH = 5,
+	SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
+	SHAPE_SPHERE = 7,
+	MAX_NUM_SHAPE_TYPES,
+};
+
+typedef struct b3Collidable b3Collidable_t;
+
+struct b3Collidable
+{
+	union {
+		int m_numChildShapes;
+		int m_bvhIndex;
+	};
+	union {
+		float m_radius;
+		int m_compoundBvhIndex;
+	};
+
+	int m_shapeType;
+	union {
+		int m_shapeIndex;
+		float m_height;
+	};
+};
+
+typedef struct b3GpuChildShape b3GpuChildShape_t;
+struct b3GpuChildShape
+{
+	b3Float4 m_childPosition;
+	b3Quat m_childOrientation;
+	union {
+		int m_shapeIndex;  //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
+		int m_capsuleAxis;
+	};
+	union {
+		float m_radius;        //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
+		int m_numChildShapes;  //used for compound shape
+	};
+	union {
+		float m_height;  //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
+		int m_collidableShapeIndex;
+	};
+	int m_shapeType;
+};
+
+struct b3CompoundOverlappingPair
+{
+	int m_bodyIndexA;
+	int m_bodyIndexB;
+	//	int	m_pairType;
+	int m_childShapeIndexA;
+	int m_childShapeIndexB;
+};
+
+#endif  //B3_COLLIDABLE_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

@@ -0,0 +1,36 @@
+#ifndef B3_CONTACT4DATA_H
+#define B3_CONTACT4DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+typedef struct b3Contact4Data b3Contact4Data_t;
+
+struct b3Contact4Data
+{
+	b3Float4 m_worldPosB[4];
+	//	b3Float4	m_localPosA[4];
+	//	b3Float4	m_localPosB[4];
+	b3Float4 m_worldNormalOnB;  //	w: m_nPoints
+	unsigned short m_restituitionCoeffCmp;
+	unsigned short m_frictionCoeffCmp;
+	int m_batchIdx;
+	int m_bodyAPtrAndSignBit;  //x:m_bodyAPtr, y:m_bodyBPtr
+	int m_bodyBPtrAndSignBit;
+
+	int m_childIndexA;
+	int m_childIndexB;
+	int m_unused1;
+	int m_unused2;
+};
+
+inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
+{
+	return (int)contact->m_worldNormalOnB.w;
+};
+
+inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
+{
+	contact->m_worldNormalOnB.w = (float)numPoints;
+};
+
+#endif  //B3_CONTACT4DATA_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h

@@ -0,0 +1,486 @@
+
+#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
+#define B3_CONTACT_CONVEX_CONVEX_SAT_H
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
+
+#define B3_MAX_VERTS 1024
+
+inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
+{
+	return b3MakeVector3(a.x + (b.x - a.x) * t,
+						 a.y + (b.y - a.y) * t,
+						 a.z + (b.z - a.z) * t,
+						 0.f);
+}
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
+{
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+	if (numVertsIn < 2)
+		return 0;
+
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
+	b3Float4 endVertex = pVtxIn[0];
+
+	ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
+
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex = pVtxIn[ve];
+
+		de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
+
+		if (ds < 0)
+		{
+			if (de < 0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+			}
+		}
+		else
+		{
+			if (de < 0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
+								 const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
+								 b3Float4* worldVertsB2, int capacityWorldVertsB2,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 //const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
+								 b3Float4* contactsOut,
+								 int contactCapacity)
+{
+	int numContactsOut = 0;
+
+	b3Float4* pVtxIn = worldVertsB1;
+	b3Float4* pVtxOut = worldVertsB2;
+
+	int numVertsIn = numWorldVertsB1;
+	int numVertsOut = 0;
+
+	int closestFaceA = -1;
+	{
+		float dmin = FLT_MAX;
+		for (int face = 0; face < hullA->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeVector3(
+				facesA[hullA->m_faceOffset + face].m_plane.x,
+				facesA[hullA->m_faceOffset + face].m_plane.y,
+				facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+			float d = b3Dot3F4(faceANormalWS, separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+			}
+		}
+	}
+	if (closestFaceA < 0)
+		return numContactsOut;
+
+	b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
+
+	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+	//int numContacts = numWorldVertsB1;
+	int numVerticesA = polyA.m_numIndices;
+	for (int e0 = 0; e0 < numVerticesA; e0++)
+	{
+		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
+		const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
+		const b3Float4 edge0 = a - b;
+		const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
+		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
+
+		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
+		b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
+		float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
+
+		b3Float4 planeNormalWS = planeNormalWS1;
+		float planeEqWS = planeEqWS1;
+
+		//clip face
+		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
+		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
+
+		//btSwap(pVtxIn,pVtxOut);
+		b3Float4* tmp = pVtxOut;
+		pVtxOut = pVtxIn;
+		pVtxIn = tmp;
+		numVertsIn = numVertsOut;
+		numVertsOut = 0;
+	}
+
+	// only keep points that are behind the witness face
+	{
+		b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+		float localPlaneEq = polyA.m_plane.w;
+		b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
+		float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
+		for (int i = 0; i < numVertsIn; i++)
+		{
+			float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
+			if (depth <= minDist)
+			{
+				depth = minDist;
+			}
+			if (numContactsOut < contactCapacity)
+			{
+				if (depth <= maxDist)
+				{
+					b3Float4 pointInWorld = pVtxIn[i];
+					//resultOut.addContactPoint(separatingNormal,point,depth);
+					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+					//printf("depth=%f\n",depth);
+				}
+			}
+			else
+			{
+				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
+			}
+		}
+	}
+
+	return numContactsOut;
+}
+
+inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
+								 const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
+								 b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
+
+								 b3Float4* contactsOut,
+								 int contactCapacity)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1 = 0;
+
+	B3_PROFILE("clipHullAgainstHull");
+
+	//float curMaxDist=maxDist;
+	int closestFaceB = -1;
+	float dmax = -FLT_MAX;
+
+	{
+		//B3_PROFILE("closestFaceB");
+		if (hullB.m_numFaces != 1)
+		{
+			//printf("wtf\n");
+		}
+		static bool once = true;
+		//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
+
+		for (int face = 0; face < hullB.m_numFaces; face++)
+		{
+#ifdef BT_DEBUG_SAT_FACE
+			if (once)
+				printf("face %d\n", face);
+			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
+			if (once)
+			{
+				for (int i = 0; i < faceB->m_numIndices; i++)
+				{
+					b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
+					printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
+				}
+			}
+#endif  //BT_DEBUG_SAT_FACE \
+	//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
+			{
+				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
+													  facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
+				const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+#ifdef BT_DEBUG_SAT_FACE
+				if (once)
+					printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
+#endif
+				float d = b3Dot3F4(WorldNormal, separatingNormal);
+				if (d > dmax)
+				{
+					dmax = d;
+					closestFaceB = face;
+				}
+			}
+		}
+		once = false;
+	}
+
+	b3Assert(closestFaceB >= 0);
+	{
+		//B3_PROFILE("worldVertsB1");
+		const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
+		}
+	}
+
+	if (closestFaceB >= 0)
+	{
+		//B3_PROFILE("clipFaceAgainstHull");
+		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
+											   posA, ornA,
+											   worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
+											   verticesA, facesA, indicesA,
+											   contactsOut, contactCapacity);
+	}
+
+	return numContactsOut;
+}
+
+inline int b3ClipHullHullSingle(
+	int bodyIndexA, int bodyIndexB,
+	const b3Float4& posA,
+	const b3Quaternion& ornA,
+	const b3Float4& posB,
+	const b3Quaternion& ornB,
+
+	int collidableIndexA, int collidableIndexB,
+
+	const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
+	b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
+	int& nContacts,
+
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+	const b3AlignedObjectArray<b3GpuFace>& facesA,
+	const b3AlignedObjectArray<int>& indicesA,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+	const b3AlignedObjectArray<b3GpuFace>& facesB,
+	const b3AlignedObjectArray<int>& indicesB,
+
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
+	const b3Vector3& sepNormalWorldSpace,
+	int maxContactCapacity)
+{
+	int contactIndex = -1;
+	b3ConvexPolyhedronData hullA, hullB;
+
+	b3Collidable colA = hostCollidablesA[collidableIndexA];
+	hullA = hostConvexDataA[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
+
+	b3Collidable colB = hostCollidablesB[collidableIndexB];
+	hullB = hostConvexDataB[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
+
+	b3Float4 contactsOut[B3_MAX_VERTS];
+	int localContactCapacity = B3_MAX_VERTS;
+
+#ifdef _WIN32
+	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
+	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
+#endif
+
+	{
+		b3Float4 worldVertsB1[B3_MAX_VERTS];
+		b3Float4 worldVertsB2[B3_MAX_VERTS];
+		int capacityWorldVerts = B3_MAX_VERTS;
+
+		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
+		int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
+		int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
+
+		b3Scalar minDist = -1;
+		b3Scalar maxDist = 0.;
+
+		b3Transform trA, trB;
+		{
+			//B3_PROFILE("b3TransformPoint computation");
+			//trA.setIdentity();
+			trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
+			trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
+
+			//trB.setIdentity();
+			trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
+			trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
+		}
+
+		b3Quaternion trAorn = trA.getRotation();
+		b3Quaternion trBorn = trB.getRotation();
+
+		int numContactsOut = b3ClipHullAgainstHull(hostNormal,
+												   hostConvexDataA.at(shapeA),
+												   hostConvexDataB.at(shapeB),
+												   (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
+												   (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
+												   worldVertsB1, worldVertsB2, capacityWorldVerts,
+												   minDist, maxDist,
+												   verticesA, facesA, indicesA,
+												   verticesB, facesB, indicesB,
+
+												   contactsOut, localContactCapacity);
+
+		if (numContactsOut > 0)
+		{
+			B3_PROFILE("overlap");
+
+			b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
+			//			b3Float4 centerOut;
+
+			b3Int4 contactIdx;
+			contactIdx.x = 0;
+			contactIdx.y = 1;
+			contactIdx.z = 2;
+			contactIdx.w = 3;
+
+			int numPoints = 0;
+
+			{
+				B3_PROFILE("extractManifold");
+				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
+			}
+
+			b3Assert(numPoints);
+
+			if (nContacts < maxContactCapacity)
+			{
+				contactIndex = nContacts;
+				globalContactOut->expand();
+				b3Contact4Data& contact = globalContactOut->at(nContacts);
+				contact.m_batchIdx = 0;  //i;
+				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
+				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
+
+				contact.m_frictionCoeffCmp = 45874;
+				contact.m_restituitionCoeffCmp = 0;
+
+				//	float distance = 0.f;
+				for (int p = 0; p < numPoints; p++)
+				{
+					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];  //check if it is actually on B
+					contact.m_worldNormalOnB = normalOnSurfaceB;
+				}
+				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
+				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
+				nContacts++;
+			}
+			else
+			{
+				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
+			}
+		}
+	}
+	return contactIndex;
+}
+
+inline int b3ContactConvexConvexSAT(
+	int pairIndex,
+	int bodyIndexA, int bodyIndexB,
+	int collidableIndexA, int collidableIndexB,
+	const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
+	const b3AlignedObjectArray<b3Collidable>& collidables,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
+	const b3AlignedObjectArray<b3Float4>& convexVertices,
+	const b3AlignedObjectArray<b3Float4>& uniqueEdges,
+	const b3AlignedObjectArray<int>& convexIndices,
+	const b3AlignedObjectArray<b3GpuFace>& faces,
+	b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
+	int& nGlobalContactsOut,
+	int maxContactCapacity)
+{
+	int contactIndex = -1;
+
+	b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+	b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
+
+	b3ConvexPolyhedronData hullA, hullB;
+
+	b3Float4 sepNormalWorldSpace;
+
+	b3Collidable colA = collidables[collidableIndexA];
+	hullA = convexShapes[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
+
+	b3Collidable colB = collidables[collidableIndexB];
+	hullB = convexShapes[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
+
+#ifdef _WIN32
+	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
+	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
+#endif
+
+	bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
+											 posA,
+											 ornA,
+											 posB,
+											 ornB,
+
+											 convexVertices, uniqueEdges, faces, convexIndices,
+											 convexVertices, uniqueEdges, faces, convexIndices,
+
+											 sepNormalWorldSpace);
+
+	if (foundSepAxis)
+	{
+		contactIndex = b3ClipHullHullSingle(
+			bodyIndexA, bodyIndexB,
+			posA, ornA,
+			posB, ornB,
+			collidableIndexA, collidableIndexB,
+			&rigidBodies,
+			&globalContactsOut,
+			nGlobalContactsOut,
+
+			convexShapes,
+			convexShapes,
+
+			convexVertices,
+			uniqueEdges,
+			faces,
+			convexIndices,
+
+			convexVertices,
+			uniqueEdges,
+			faces,
+			convexIndices,
+
+			collidables,
+			collidables,
+			sepNormalWorldSpace,
+			maxContactCapacity);
+	}
+
+	return contactIndex;
+}
+
+#endif  //B3_CONTACT_CONVEX_CONVEX_SAT_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

@@ -0,0 +1,153 @@
+
+#ifndef B3_CONTACT_SPHERE_SPHERE_H
+#define B3_CONTACT_SPHERE_SPHERE_H
+
+void computeContactSphereConvex(int pairIndex,
+								int bodyIndexA, int bodyIndexB,
+								int collidableIndexA, int collidableIndexB,
+								const b3RigidBodyData* rigidBodies,
+								const b3Collidable* collidables,
+								const b3ConvexPolyhedronData* convexShapes,
+								const b3Vector3* convexVertices,
+								const int* convexIndices,
+								const b3GpuFace* faces,
+								b3Contact4* globalContactsOut,
+								int& nGlobalContactsOut,
+								int maxContactCapacity)
+{
+	float radius = collidables[collidableIndexA].m_radius;
+	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
+
+	float4 pos = rigidBodies[bodyIndexB].m_pos;
+
+	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
+
+	b3Transform tr;
+	tr.setIdentity();
+	tr.setOrigin(pos);
+	tr.setRotation(quat);
+	b3Transform trInv = tr.inverse();
+
+	float4 spherePos = trInv(spherePos1);
+
+	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+	int numFaces = convexShapes[shapeIndex].m_numFaces;
+	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
+	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
+	float minDist = -1000000.f;  // TODO: What is the largest/smallest float?
+	bool bCollide = true;
+	int region = -1;
+	float4 localHitNormal;
+	for (int f = 0; f < numFaces; f++)
+	{
+		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
+		float4 planeEqn;
+		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+		float4 n1 = localPlaneNormal;  //quatRotate(quat,localPlaneNormal);
+		planeEqn = n1;
+		planeEqn[3] = face.m_plane.w;
+
+		float4 pntReturn;
+		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
+
+		if (dist > radius)
+		{
+			bCollide = false;
+			break;
+		}
+
+		if (dist > 0)
+		{
+			//might hit an edge or vertex
+			b3Vector3 out;
+
+			bool isInPoly = IsPointInPolygon(spherePos,
+											 &face,
+											 &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+											 convexIndices,
+											 &out);
+			if (isInPoly)
+			{
+				if (dist > minDist)
+				{
+					minDist = dist;
+					closestPnt = pntReturn;
+					localHitNormal = planeEqn;
+					region = 1;
+				}
+			}
+			else
+			{
+				b3Vector3 tmp = spherePos - out;
+				b3Scalar l2 = tmp.length2();
+				if (l2 < radius * radius)
+				{
+					dist = b3Sqrt(l2);
+					if (dist > minDist)
+					{
+						minDist = dist;
+						closestPnt = out;
+						localHitNormal = tmp / dist;
+						region = 2;
+					}
+				}
+				else
+				{
+					bCollide = false;
+					break;
+				}
+			}
+		}
+		else
+		{
+			if (dist > minDist)
+			{
+				minDist = dist;
+				closestPnt = pntReturn;
+				localHitNormal = planeEqn;
+				region = 3;
+			}
+		}
+	}
+	static int numChecks = 0;
+	numChecks++;
+
+	if (bCollide && minDist > -10000)
+	{
+		float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal;  //-hitNormalWorld;
+		float4 pOnB1 = tr(closestPnt);
+		//printf("dist ,%f,",minDist);
+		float actualDepth = minDist - radius;
+		if (actualDepth < 0)
+		{
+			//printf("actualDepth = ,%f,", actualDepth);
+			//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+			//printf("region=,%d,\n", region);
+			pOnB1[3] = actualDepth;
+
+			int dstIdx;
+			//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+
+			if (nGlobalContactsOut < maxContactCapacity)
+			{
+				dstIdx = nGlobalContactsOut;
+				nGlobalContactsOut++;
+
+				b3Contact4* c = &globalContactsOut[dstIdx];
+				c->m_worldNormalOnB = normalOnSurfaceB1;
+				c->setFrictionCoeff(0.7);
+				c->setRestituitionCoeff(0.f);
+
+				c->m_batchIdx = pairIndex;
+				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
+				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
+				c->m_worldPosB[0] = pOnB1;
+				int numPoints = 1;
+				c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+			}  //if (dstIdx < numPairs)
+		}
+	}  //if (hasCollision)
+}
+#endif  //B3_CONTACT_SPHERE_SPHERE_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h

@@ -0,0 +1,38 @@
+
+#ifndef B3_CONVEX_POLYHEDRON_DATA_H
+#define B3_CONVEX_POLYHEDRON_DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+typedef struct b3GpuFace b3GpuFace_t;
+struct b3GpuFace
+{
+	b3Float4 m_plane;
+	int m_indexOffset;
+	int m_numIndices;
+	int m_unusedPadding1;
+	int m_unusedPadding2;
+};
+
+typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
+
+struct b3ConvexPolyhedronData
+{
+	b3Float4 m_localCenter;
+	b3Float4 m_extents;
+	b3Float4 mC;
+	b3Float4 mE;
+
+	float m_radius;
+	int m_faceOffset;
+	int m_numFaces;
+	int m_numVertices;
+
+	int m_vertexOffset;
+	int m_uniqueEdgesOffset;
+	int m_numUniqueEdges;
+	int m_unused;
+};
+
+#endif  //B3_CONVEX_POLYHEDRON_DATA_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h

@@ -0,0 +1,797 @@
+#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
+#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
+#define B3_TRIANGLE_NUM_CONVEX_FACES 5
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
+					  const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
+{
+	min[0] = FLT_MAX;
+	max[0] = -FLT_MAX;
+	int numVerts = hull->m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
+	float offset = b3Dot(pos, *dir);
+	for (int i = 0; i < numVerts; i++)
+	{
+		float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
+		if (dp < min[0])
+			min[0] = dp;
+		if (dp > max[0])
+			max[0] = dp;
+	}
+	if (min[0] > max[0])
+	{
+		float tmp = min[0];
+		min[0] = max[0];
+		max[0] = tmp;
+	}
+	min[0] += offset;
+	max[0] += offset;
+}
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+						  b3Float4ConstArg posA, b3QuatConstArg ornA,
+						  b3Float4ConstArg posB, b3QuatConstArg ornB,
+						  b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
+{
+	float Min0, Max0;
+	float Min1, Max1;
+	b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
+	b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
+
+	if (Max0 < Min1 || Max1 < Min0)
+		return false;
+
+	float d0 = Max0 - Min1;
+	float d1 = Max1 - Min0;
+	*depth = d0 < d1 ? d0 : d1;
+	return true;
+}
+
+bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+						  b3Float4ConstArg posA1,
+						  b3QuatConstArg ornA,
+						  b3Float4ConstArg posB1,
+						  b3QuatConstArg ornB,
+						  b3Float4ConstArg DeltaC2,
+
+						  const b3Float4* verticesA,
+						  const b3Float4* uniqueEdgesA,
+						  const b3GpuFace* facesA,
+						  const int* indicesA,
+
+						  __global const b3Float4* verticesB,
+						  __global const b3Float4* uniqueEdgesB,
+						  __global const b3GpuFace* facesB,
+						  __global const int* indicesB,
+						  b3Float4* sep,
+						  float* dmin)
+{
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+	/*
+	static int maxFaceVertex = 0;
+
+	int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
+	curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
+
+	if (curFaceVertexAB>maxFaceVertex)
+	{
+		maxFaceVertex = curFaceVertexAB;
+		printf("curFaceVertexAB = %d\n",curFaceVertexAB);
+		printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
+		printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
+		printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
+	}
+*/
+
+	int curPlaneTests = 0;
+	{
+		int numFacesA = hullA->m_numFaces;
+		// Test normals from hullA
+		for (int i = 0; i < numFacesA; i++)
+		{
+			const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
+			b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
+			if (b3Dot(DeltaC2, faceANormalWS) < 0)
+				faceANormalWS *= -1.f;
+			curPlaneTests++;
+			float d;
+			if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
+				return false;
+			if (d < *dmin)
+			{
+				*dmin = d;
+				*sep = faceANormalWS;
+			}
+		}
+	}
+	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+b3Vector3 unitSphere162[] =
+	{
+		b3MakeVector3(0.000000, -1.000000, 0.000000),
+		b3MakeVector3(0.203181, -0.967950, 0.147618),
+		b3MakeVector3(-0.077607, -0.967950, 0.238853),
+		b3MakeVector3(0.723607, -0.447220, 0.525725),
+		b3MakeVector3(0.609547, -0.657519, 0.442856),
+		b3MakeVector3(0.812729, -0.502301, 0.295238),
+		b3MakeVector3(-0.251147, -0.967949, 0.000000),
+		b3MakeVector3(-0.077607, -0.967950, -0.238853),
+		b3MakeVector3(0.203181, -0.967950, -0.147618),
+		b3MakeVector3(0.860698, -0.251151, 0.442858),
+		b3MakeVector3(-0.276388, -0.447220, 0.850649),
+		b3MakeVector3(-0.029639, -0.502302, 0.864184),
+		b3MakeVector3(-0.155215, -0.251152, 0.955422),
+		b3MakeVector3(-0.894426, -0.447216, 0.000000),
+		b3MakeVector3(-0.831051, -0.502299, 0.238853),
+		b3MakeVector3(-0.956626, -0.251149, 0.147618),
+		b3MakeVector3(-0.276388, -0.447220, -0.850649),
+		b3MakeVector3(-0.483971, -0.502302, -0.716565),
+		b3MakeVector3(-0.436007, -0.251152, -0.864188),
+		b3MakeVector3(0.723607, -0.447220, -0.525725),
+		b3MakeVector3(0.531941, -0.502302, -0.681712),
+		b3MakeVector3(0.687159, -0.251152, -0.681715),
+		b3MakeVector3(0.687159, -0.251152, 0.681715),
+		b3MakeVector3(-0.436007, -0.251152, 0.864188),
+		b3MakeVector3(-0.956626, -0.251149, -0.147618),
+		b3MakeVector3(-0.155215, -0.251152, -0.955422),
+		b3MakeVector3(0.860698, -0.251151, -0.442858),
+		b3MakeVector3(0.276388, 0.447220, 0.850649),
+		b3MakeVector3(0.483971, 0.502302, 0.716565),
+		b3MakeVector3(0.232822, 0.657519, 0.716563),
+		b3MakeVector3(-0.723607, 0.447220, 0.525725),
+		b3MakeVector3(-0.531941, 0.502302, 0.681712),
+		b3MakeVector3(-0.609547, 0.657519, 0.442856),
+		b3MakeVector3(-0.723607, 0.447220, -0.525725),
+		b3MakeVector3(-0.812729, 0.502301, -0.295238),
+		b3MakeVector3(-0.609547, 0.657519, -0.442856),
+		b3MakeVector3(0.276388, 0.447220, -0.850649),
+		b3MakeVector3(0.029639, 0.502302, -0.864184),
+		b3MakeVector3(0.232822, 0.657519, -0.716563),
+		b3MakeVector3(0.894426, 0.447216, 0.000000),
+		b3MakeVector3(0.831051, 0.502299, -0.238853),
+		b3MakeVector3(0.753442, 0.657515, 0.000000),
+		b3MakeVector3(-0.232822, -0.657519, 0.716563),
+		b3MakeVector3(-0.162456, -0.850654, 0.499995),
+		b3MakeVector3(0.052790, -0.723612, 0.688185),
+		b3MakeVector3(0.138199, -0.894429, 0.425321),
+		b3MakeVector3(0.262869, -0.525738, 0.809012),
+		b3MakeVector3(0.361805, -0.723611, 0.587779),
+		b3MakeVector3(0.531941, -0.502302, 0.681712),
+		b3MakeVector3(0.425323, -0.850654, 0.309011),
+		b3MakeVector3(0.812729, -0.502301, -0.295238),
+		b3MakeVector3(0.609547, -0.657519, -0.442856),
+		b3MakeVector3(0.850648, -0.525736, 0.000000),
+		b3MakeVector3(0.670817, -0.723611, -0.162457),
+		b3MakeVector3(0.670817, -0.723610, 0.162458),
+		b3MakeVector3(0.425323, -0.850654, -0.309011),
+		b3MakeVector3(0.447211, -0.894428, 0.000001),
+		b3MakeVector3(-0.753442, -0.657515, 0.000000),
+		b3MakeVector3(-0.525730, -0.850652, 0.000000),
+		b3MakeVector3(-0.638195, -0.723609, 0.262864),
+		b3MakeVector3(-0.361801, -0.894428, 0.262864),
+		b3MakeVector3(-0.688189, -0.525736, 0.499997),
+		b3MakeVector3(-0.447211, -0.723610, 0.525729),
+		b3MakeVector3(-0.483971, -0.502302, 0.716565),
+		b3MakeVector3(-0.232822, -0.657519, -0.716563),
+		b3MakeVector3(-0.162456, -0.850654, -0.499995),
+		b3MakeVector3(-0.447211, -0.723611, -0.525727),
+		b3MakeVector3(-0.361801, -0.894429, -0.262863),
+		b3MakeVector3(-0.688189, -0.525736, -0.499997),
+		b3MakeVector3(-0.638195, -0.723609, -0.262863),
+		b3MakeVector3(-0.831051, -0.502299, -0.238853),
+		b3MakeVector3(0.361804, -0.723612, -0.587779),
+		b3MakeVector3(0.138197, -0.894429, -0.425321),
+		b3MakeVector3(0.262869, -0.525738, -0.809012),
+		b3MakeVector3(0.052789, -0.723611, -0.688186),
+		b3MakeVector3(-0.029639, -0.502302, -0.864184),
+		b3MakeVector3(0.956626, 0.251149, 0.147618),
+		b3MakeVector3(0.956626, 0.251149, -0.147618),
+		b3MakeVector3(0.951058, -0.000000, 0.309013),
+		b3MakeVector3(1.000000, 0.000000, 0.000000),
+		b3MakeVector3(0.947213, -0.276396, 0.162458),
+		b3MakeVector3(0.951058, 0.000000, -0.309013),
+		b3MakeVector3(0.947213, -0.276396, -0.162458),
+		b3MakeVector3(0.155215, 0.251152, 0.955422),
+		b3MakeVector3(0.436007, 0.251152, 0.864188),
+		b3MakeVector3(-0.000000, -0.000000, 1.000000),
+		b3MakeVector3(0.309017, 0.000000, 0.951056),
+		b3MakeVector3(0.138199, -0.276398, 0.951055),
+		b3MakeVector3(0.587786, 0.000000, 0.809017),
+		b3MakeVector3(0.447216, -0.276398, 0.850648),
+		b3MakeVector3(-0.860698, 0.251151, 0.442858),
+		b3MakeVector3(-0.687159, 0.251152, 0.681715),
+		b3MakeVector3(-0.951058, -0.000000, 0.309013),
+		b3MakeVector3(-0.809018, 0.000000, 0.587783),
+		b3MakeVector3(-0.861803, -0.276396, 0.425324),
+		b3MakeVector3(-0.587786, 0.000000, 0.809017),
+		b3MakeVector3(-0.670819, -0.276397, 0.688191),
+		b3MakeVector3(-0.687159, 0.251152, -0.681715),
+		b3MakeVector3(-0.860698, 0.251151, -0.442858),
+		b3MakeVector3(-0.587786, -0.000000, -0.809017),
+		b3MakeVector3(-0.809018, -0.000000, -0.587783),
+		b3MakeVector3(-0.670819, -0.276397, -0.688191),
+		b3MakeVector3(-0.951058, 0.000000, -0.309013),
+		b3MakeVector3(-0.861803, -0.276396, -0.425324),
+		b3MakeVector3(0.436007, 0.251152, -0.864188),
+		b3MakeVector3(0.155215, 0.251152, -0.955422),
+		b3MakeVector3(0.587786, -0.000000, -0.809017),
+		b3MakeVector3(0.309017, -0.000000, -0.951056),
+		b3MakeVector3(0.447216, -0.276398, -0.850648),
+		b3MakeVector3(0.000000, 0.000000, -1.000000),
+		b3MakeVector3(0.138199, -0.276398, -0.951055),
+		b3MakeVector3(0.670820, 0.276396, 0.688190),
+		b3MakeVector3(0.809019, -0.000002, 0.587783),
+		b3MakeVector3(0.688189, 0.525736, 0.499997),
+		b3MakeVector3(0.861804, 0.276394, 0.425323),
+		b3MakeVector3(0.831051, 0.502299, 0.238853),
+		b3MakeVector3(-0.447216, 0.276397, 0.850649),
+		b3MakeVector3(-0.309017, -0.000001, 0.951056),
+		b3MakeVector3(-0.262869, 0.525738, 0.809012),
+		b3MakeVector3(-0.138199, 0.276397, 0.951055),
+		b3MakeVector3(0.029639, 0.502302, 0.864184),
+		b3MakeVector3(-0.947213, 0.276396, -0.162458),
+		b3MakeVector3(-1.000000, 0.000001, 0.000000),
+		b3MakeVector3(-0.850648, 0.525736, -0.000000),
+		b3MakeVector3(-0.947213, 0.276397, 0.162458),
+		b3MakeVector3(-0.812729, 0.502301, 0.295238),
+		b3MakeVector3(-0.138199, 0.276397, -0.951055),
+		b3MakeVector3(-0.309016, -0.000000, -0.951057),
+		b3MakeVector3(-0.262869, 0.525738, -0.809012),
+		b3MakeVector3(-0.447215, 0.276397, -0.850649),
+		b3MakeVector3(-0.531941, 0.502302, -0.681712),
+		b3MakeVector3(0.861804, 0.276396, -0.425322),
+		b3MakeVector3(0.809019, 0.000000, -0.587782),
+		b3MakeVector3(0.688189, 0.525736, -0.499997),
+		b3MakeVector3(0.670821, 0.276397, -0.688189),
+		b3MakeVector3(0.483971, 0.502302, -0.716565),
+		b3MakeVector3(0.077607, 0.967950, 0.238853),
+		b3MakeVector3(0.251147, 0.967949, 0.000000),
+		b3MakeVector3(0.000000, 1.000000, 0.000000),
+		b3MakeVector3(0.162456, 0.850654, 0.499995),
+		b3MakeVector3(0.361800, 0.894429, 0.262863),
+		b3MakeVector3(0.447209, 0.723612, 0.525728),
+		b3MakeVector3(0.525730, 0.850652, 0.000000),
+		b3MakeVector3(0.638194, 0.723610, 0.262864),
+		b3MakeVector3(-0.203181, 0.967950, 0.147618),
+		b3MakeVector3(-0.425323, 0.850654, 0.309011),
+		b3MakeVector3(-0.138197, 0.894430, 0.425320),
+		b3MakeVector3(-0.361804, 0.723612, 0.587778),
+		b3MakeVector3(-0.052790, 0.723612, 0.688185),
+		b3MakeVector3(-0.203181, 0.967950, -0.147618),
+		b3MakeVector3(-0.425323, 0.850654, -0.309011),
+		b3MakeVector3(-0.447210, 0.894429, 0.000000),
+		b3MakeVector3(-0.670817, 0.723611, -0.162457),
+		b3MakeVector3(-0.670817, 0.723611, 0.162457),
+		b3MakeVector3(0.077607, 0.967950, -0.238853),
+		b3MakeVector3(0.162456, 0.850654, -0.499995),
+		b3MakeVector3(-0.138197, 0.894430, -0.425320),
+		b3MakeVector3(-0.052790, 0.723612, -0.688185),
+		b3MakeVector3(-0.361804, 0.723612, -0.587778),
+		b3MakeVector3(0.361800, 0.894429, -0.262863),
+		b3MakeVector3(0.638194, 0.723610, -0.262864),
+		b3MakeVector3(0.447209, 0.723612, -0.525728)};
+
+bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+								  b3Float4ConstArg posA1,
+								  b3QuatConstArg ornA,
+								  b3Float4ConstArg posB1,
+								  b3QuatConstArg ornB,
+								  b3Float4ConstArg DeltaC2,
+								  const b3Float4* verticesA,
+								  const b3Float4* uniqueEdgesA,
+								  const b3GpuFace* facesA,
+								  const int* indicesA,
+								  __global const b3Float4* verticesB,
+								  __global const b3Float4* uniqueEdgesB,
+								  __global const b3GpuFace* facesB,
+								  __global const int* indicesB,
+								  b3Float4* sep,
+								  float* dmin,
+								  bool searchAllEdgeEdge)
+{
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+
+	//	int curPlaneTests=0;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	static int maxEdgeTests = 0;
+	int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
+	if (curEdgeTests > maxEdgeTests)
+	{
+		maxEdgeTests = curEdgeTests;
+		printf("maxEdgeTests = %d\n", maxEdgeTests);
+		printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
+		printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
+	}
+
+	if (searchAllEdgeEdge)
+	{
+		for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
+		{
+			const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
+			b3Float4 edge0World = b3QuatRotate(ornA, edge0);
+
+			for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
+			{
+				const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
+				b3Float4 edge1World = b3QuatRotate(ornB, edge1);
+
+				b3Float4 crossje = b3Cross(edge0World, edge1World);
+
+				curEdgeEdge++;
+				if (!b3IsAlmostZero(crossje))
+				{
+					crossje = b3Normalized(crossje);
+					if (b3Dot(DeltaC2, crossje) < 0)
+						crossje *= -1.f;
+
+					float dist;
+					bool result = true;
+					{
+						float Min0, Max0;
+						float Min1, Max1;
+						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
+						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
+
+						if (Max0 < Min1 || Max1 < Min0)
+							return false;
+
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0 < d1 ? d0 : d1;
+						result = true;
+					}
+
+					if (dist < *dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
+		//printf("numDirections =%d\n",numDirections );
+
+		for (int i = 0; i < numDirections; i++)
+		{
+			b3Float4 crossje = unitSphere162[i];
+			{
+				//if (b3Dot(DeltaC2,crossje)>0)
+				{
+					float dist;
+					bool result = true;
+					{
+						float Min0, Max0;
+						float Min1, Max1;
+						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
+						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
+
+						if (Max0 < Min1 || Max1 < Min0)
+							return false;
+
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0 < d1 ? d0 : d1;
+						result = true;
+					}
+
+					if (dist < *dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+		}
+	}
+
+	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
+							   __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
+							   b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
+							   __global b3Float4* worldVertsA1,
+							   __global b3Float4* worldNormalsA1,
+							   __global b3Float4* worldVertsB1,
+							   int capacityWorldVerts,
+							   const float minDist, float maxDist,
+							   __global const b3Float4* verticesA,
+							   __global const b3GpuFace_t* facesA,
+							   __global const int* indicesA,
+							   __global const b3Float4* verticesB,
+							   __global const b3GpuFace_t* facesB,
+							   __global const int* indicesB,
+
+							   __global b3Int4* clippingFaces, int pairIndex)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1 = 0;
+
+	int closestFaceB = -1;
+	float dmax = -FLT_MAX;
+
+	{
+		for (int face = 0; face < hullB->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
+												 facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
+			const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+			float d = b3Dot(WorldNormal, separatingNormal);
+			if (d > dmax)
+			{
+				dmax = d;
+				closestFaceB = face;
+			}
+		}
+	}
+
+	{
+		const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+			worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
+		}
+	}
+
+	int closestFaceA = -1;
+	{
+		float dmin = FLT_MAX;
+		for (int face = 0; face < hullA->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(
+				facesA[hullA->m_faceOffset + face].m_plane.x,
+				facesA[hullA->m_faceOffset + face].m_plane.y,
+				facesA[hullA->m_faceOffset + face].m_plane.z,
+				0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+			float d = b3Dot(faceANormalWS, separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+				worldNormalsA1[pairIndex] = faceANormalWS;
+			}
+		}
+	}
+
+	int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
+	for (int e0 = 0; e0 < numVerticesA; e0++)
+	{
+		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
+		worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
+	}
+
+	clippingFaces[pairIndex].x = closestFaceA;
+	clippingFaces[pairIndex].y = closestFaceB;
+	clippingFaces[pairIndex].z = numVerticesA;
+	clippingFaces[pairIndex].w = numWorldVertsB1;
+
+	return numContactsOut;
+}
+
+__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
+												__global const b3RigidBodyData* rigidBodies,
+												__global const b3Collidable* collidables,
+												__global const b3ConvexPolyhedronData* convexShapes,
+												__global const b3Float4* vertices,
+												__global const b3Float4* uniqueEdges,
+												__global const b3GpuFace* faces,
+												__global const int* indices,
+												__global const b3GpuChildShape* gpuChildShapes,
+												__global b3Aabb* aabbs,
+												__global b3Float4* concaveSeparatingNormalsOut,
+												__global b3Int4* clippingFacesOut,
+												__global b3Vector3* worldVertsA1Out,
+												__global b3Vector3* worldNormalsA1Out,
+												__global b3Vector3* worldVertsB1Out,
+												__global int* hasSeparatingNormals,
+												int vertexFaceCapacity,
+												int numConcavePairs,
+												int pairIdx)
+{
+	int i = pairIdx;
+	/*	int i = get_global_id(0);
+	if (i>=numConcavePairs)
+		return;
+	int pairIdx = i;
+	*/
+
+	int bodyIndexA = concavePairs[i].x;
+	int bodyIndexB = concavePairs[i].y;
+
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
+		collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
+	{
+		concavePairs[pairIdx].w = -1;
+		return;
+	}
+
+	hasSeparatingNormals[i] = 0;
+
+	//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
+	int numActualConcaveConvexTests = 0;
+
+	int f = concavePairs[i].z;
+
+	bool overlap = false;
+
+	b3ConvexPolyhedronData convexPolyhedronA;
+
+	//add 3 vertices of the triangle
+	convexPolyhedronA.m_numVertices = 3;
+	convexPolyhedronA.m_vertexOffset = 0;
+	b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+
+	b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
+	b3Aabb triAabb;
+	triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
+	triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
+
+	b3Float4 verticesA[3];
+	for (int i = 0; i < 3; i++)
+	{
+		int index = indices[face.m_indexOffset + i];
+		b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
+		verticesA[i] = vert;
+		localCenter += vert;
+
+		triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
+		triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
+	}
+
+	overlap = true;
+	overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
+	overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
+	overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
+
+	if (overlap)
+	{
+		float dmin = FLT_MAX;
+		int hasSeparatingAxis = 5;
+		b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
+
+		//	int localCC=0;
+		numActualConcaveConvexTests++;
+
+		//a triangle has 3 unique edges
+		convexPolyhedronA.m_numUniqueEdges = 3;
+		convexPolyhedronA.m_uniqueEdgesOffset = 0;
+		b3Float4 uniqueEdgesA[3];
+
+		uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
+		uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
+		uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
+
+		convexPolyhedronA.m_faceOffset = 0;
+
+		b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+
+		b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
+		int indicesA[3 + 3 + 2 + 2 + 2];
+		int curUsedIndices = 0;
+		int fidx = 0;
+
+		//front size of triangle
+		{
+			facesA[fidx].m_indexOffset = curUsedIndices;
+			indicesA[0] = 0;
+			indicesA[1] = 1;
+			indicesA[2] = 2;
+			curUsedIndices += 3;
+			float c = face.m_plane.w;
+			facesA[fidx].m_plane.x = normal.x;
+			facesA[fidx].m_plane.y = normal.y;
+			facesA[fidx].m_plane.z = normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices = 3;
+		}
+		fidx++;
+		//back size of triangle
+		{
+			facesA[fidx].m_indexOffset = curUsedIndices;
+			indicesA[3] = 2;
+			indicesA[4] = 1;
+			indicesA[5] = 0;
+			curUsedIndices += 3;
+			float c = b3Dot(normal, verticesA[0]);
+			//	float c1 = -face.m_plane.w;
+			facesA[fidx].m_plane.x = -normal.x;
+			facesA[fidx].m_plane.y = -normal.y;
+			facesA[fidx].m_plane.z = -normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices = 3;
+		}
+		fidx++;
+
+		bool addEdgePlanes = true;
+		if (addEdgePlanes)
+		{
+			int numVertices = 3;
+			int prevVertex = numVertices - 1;
+			for (int i = 0; i < numVertices; i++)
+			{
+				b3Float4 v0 = verticesA[i];
+				b3Float4 v1 = verticesA[prevVertex];
+
+				b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
+				float c = -b3Dot(edgeNormal, v0);
+
+				facesA[fidx].m_numIndices = 2;
+				facesA[fidx].m_indexOffset = curUsedIndices;
+				indicesA[curUsedIndices++] = i;
+				indicesA[curUsedIndices++] = prevVertex;
+
+				facesA[fidx].m_plane.x = edgeNormal.x;
+				facesA[fidx].m_plane.y = edgeNormal.y;
+				facesA[fidx].m_plane.z = edgeNormal.z;
+				facesA[fidx].m_plane.w = c;
+				fidx++;
+				prevVertex = i;
+			}
+		}
+		convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
+		convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
+
+		b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+		posA.w = 0.f;
+		b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+		posB.w = 0.f;
+
+		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+		b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
+
+		///////////////////
+		///compound shape support
+
+		if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			int compoundChild = concavePairs[pairIdx].w;
+			int childShapeIndexB = compoundChild;  //collidables[collidableIndexB].m_shapeIndex+compoundChild;
+			int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
+			b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
+			b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
+			b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
+			b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
+			posB = newPosB;
+			ornB = newOrnB;
+			shapeIndexB = collidables[childColIndexB].m_shapeIndex;
+		}
+		//////////////////
+
+		b3Float4 c0local = convexPolyhedronA.m_localCenter;
+		b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+		b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
+		b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
+		const b3Float4 DeltaC2 = c0 - c1;
+
+		bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
+										 posA, ornA,
+										 posB, ornB,
+										 DeltaC2,
+										 verticesA, uniqueEdgesA, facesA, indicesA,
+										 vertices, uniqueEdges, faces, indices,
+										 &sepAxis, &dmin);
+		hasSeparatingAxis = 4;
+		if (!sepA)
+		{
+			hasSeparatingAxis = 0;
+		}
+		else
+		{
+			bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
+											 posB, ornB,
+											 posA, ornA,
+											 DeltaC2,
+											 vertices, uniqueEdges, faces, indices,
+											 verticesA, uniqueEdgesA, facesA, indicesA,
+											 &sepAxis, &dmin);
+
+			if (!sepB)
+			{
+				hasSeparatingAxis = 0;
+			}
+			else
+			{
+				bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
+														  posA, ornA,
+														  posB, ornB,
+														  DeltaC2,
+														  verticesA, uniqueEdgesA, facesA, indicesA,
+														  vertices, uniqueEdges, faces, indices,
+														  &sepAxis, &dmin, true);
+
+				if (!sepEE)
+				{
+					hasSeparatingAxis = 0;
+				}
+				else
+				{
+					hasSeparatingAxis = 1;
+				}
+			}
+		}
+
+		if (hasSeparatingAxis)
+		{
+			hasSeparatingNormals[i] = 1;
+			sepAxis.w = dmin;
+			concaveSeparatingNormalsOut[pairIdx] = sepAxis;
+
+			//now compute clipping faces A and B, and world-space clipping vertices A and B...
+
+			float minDist = -1e30f;
+			float maxDist = 0.02f;
+
+			b3FindClippingFaces(sepAxis,
+								&convexPolyhedronA,
+								&convexShapes[shapeIndexB],
+								posA, ornA,
+								posB, ornB,
+								worldVertsA1Out,
+								worldNormalsA1Out,
+								worldVertsB1Out,
+								vertexFaceCapacity,
+								minDist, maxDist,
+								verticesA,
+								facesA,
+								indicesA,
+
+								vertices,
+								faces,
+								indices,
+								clippingFacesOut, pairIdx);
+		}
+		else
+		{
+			//mark this pair as in-active
+			concavePairs[pairIdx].w = -1;
+		}
+	}
+	else
+	{
+		//mark this pair as in-active
+		concavePairs[pairIdx].w = -1;
+	}
+}
+
+#endif  //B3_FIND_CONCAVE_SEPARATING_AXIS_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

@@ -0,0 +1,197 @@
+#ifndef B3_FIND_SEPARATING_AXIS_H
+#define B3_FIND_SEPARATING_AXIS_H
+
+inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
+{
+	min = FLT_MAX;
+	max = -FLT_MAX;
+	int numVerts = hull.m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
+
+	b3Scalar offset = b3Dot3F4(pos, dir);
+
+	for (int i = 0; i < numVerts; i++)
+	{
+		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
+		//b3Scalar dp = pt.dot(dir);
+		//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
+		b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
+		//b3Assert(dp==dpL);
+		if (dp < min) min = dp;
+		if (dp > max) max = dp;
+	}
+	if (min > max)
+	{
+		b3Scalar tmp = min;
+		min = max;
+		max = tmp;
+	}
+	min += offset;
+	max += offset;
+}
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+						  const b3Float4& posA, const b3Quaternion& ornA,
+						  const b3Float4& posB, const b3Quaternion& ornB,
+						  const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
+{
+	b3Scalar Min0, Max0;
+	b3Scalar Min1, Max1;
+	b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
+	b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
+
+	if (Max0 < Min1 || Max1 < Min0)
+		return false;
+
+	b3Scalar d0 = Max0 - Min1;
+	b3Assert(d0 >= 0.0f);
+	b3Scalar d1 = Max1 - Min0;
+	b3Assert(d1 >= 0.0f);
+	depth = d0 < d1 ? d0 : d1;
+	return true;
+}
+
+inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA1,
+								 const b3Quaternion& ornA,
+								 const b3Float4& posB1,
+								 const b3Quaternion& ornB,
+								 const b3AlignedObjectArray<b3Vector3>& verticesA,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+								 const b3AlignedObjectArray<b3GpuFace>& facesA,
+								 const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Vector3>& verticesB,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+								 const b3AlignedObjectArray<b3GpuFace>& facesB,
+								 const b3AlignedObjectArray<int>& indicesB,
+
+								 b3Vector3& sep)
+{
+	B3_PROFILE("findSeparatingAxis");
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+	//#ifdef TEST_INTERNAL_OBJECTS
+	b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
+
+	b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+	b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
+	b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
+	const b3Float4 deltaC2 = c0 - c1;
+	//#endif
+
+	b3Scalar dmin = FLT_MAX;
+	int curPlaneTests = 0;
+
+	int numFacesA = hullA.m_numFaces;
+	// Test normals from hullA
+	for (int i = 0; i < numFacesA; i++)
+	{
+		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
+		b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
+
+		if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
+			faceANormalWS *= -1.f;
+
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		b3Scalar d;
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
+			return false;
+
+		if (d < dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)faceANormalWS;
+		}
+	}
+
+	int numFacesB = hullB.m_numFaces;
+	// Test normals from hullB
+	for (int i = 0; i < numFacesB; i++)
+	{
+		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
+		b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
+
+		if (b3Dot3F4(deltaC2, WorldNormal) < 0)
+		{
+			WorldNormal *= -1.f;
+		}
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		b3Scalar d;
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
+			return false;
+
+		if (d < dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)WorldNormal;
+		}
+	}
+
+	//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
+	{
+		const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
+		b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
+
+		for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
+		{
+			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
+			b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
+
+			b3Float4 crossje = b3Cross3(edge0World, edge1World);
+
+			curEdgeEdge++;
+			if (!b3IsAlmostZero((b3Vector3&)crossje))
+			{
+				crossje = b3FastNormalized3(crossje);
+				if (b3Dot3F4(deltaC2, crossje) < 0)
+					crossje *= -1.f;
+
+#ifdef TEST_INTERNAL_OBJECTS
+				gExpectedNbTests++;
+				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
+					continue;
+				gActualNbTests++;
+#endif
+
+				b3Scalar dist;
+				if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
+					return false;
+
+				if (dist < dmin)
+				{
+					dmin = dist;
+					sep = (b3Vector3&)crossje;
+				}
+			}
+		}
+	}
+
+	if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
+		sep = -sep;
+
+	return true;
+}
+
+#endif  //B3_FIND_SEPARATING_AXIS_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

@@ -0,0 +1,888 @@
+
+/***
+ * ---------------------------------
+ * Copyright (c)2012 Daniel Fiser <[email protected]>
+ *
+ *  This file was ported from mpr.c file, part of libccd.
+ *  The Minkoski Portal Refinement implementation was ported 
+ *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
+ *  at http://github.com/erwincoumans/bullet3
+ *
+ *  Distributed under the OSI-approved BSD License (the "License");
+ *  see <http://www.opensource.org/licenses/bsd-license.php>.
+ *  This software is distributed WITHOUT ANY WARRANTY; without even the
+ *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *  See the License for more information.
+ */
+
+#ifndef B3_MPR_PENETRATION_H
+#define B3_MPR_PENETRATION_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+
+#ifdef __cplusplus
+#define B3_MPR_SQRT sqrtf
+#else
+#define B3_MPR_SQRT sqrt
+#endif
+#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
+#define B3_MPR_FABS fabs
+
+#define B3_MPR_TOLERANCE 1E-6f
+#define B3_MPR_MAX_ITERATIONS 1000
+
+struct _b3MprSupport_t
+{
+	b3Float4 v;   //!< Support point in minkowski sum
+	b3Float4 v1;  //!< Support point in obj1
+	b3Float4 v2;  //!< Support point in obj2
+};
+typedef struct _b3MprSupport_t b3MprSupport_t;
+
+struct _b3MprSimplex_t
+{
+	b3MprSupport_t ps[4];
+	int last;  //!< index of last added point
+};
+typedef struct _b3MprSimplex_t b3MprSimplex_t;
+
+inline b3MprSupport_t *b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
+{
+	return &s->ps[idx];
+}
+
+inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
+{
+	s->last = size - 1;
+}
+
+inline int b3MprSimplexSize(const b3MprSimplex_t *s)
+{
+	return s->last + 1;
+}
+
+inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
+{
+	// here is no check on boundaries
+	return &s->ps[idx];
+}
+
+inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
+{
+	*d = *s;
+}
+
+inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
+{
+	b3MprSupportCopy(s->ps + pos, a);
+}
+
+inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
+{
+	b3MprSupport_t supp;
+
+	b3MprSupportCopy(&supp, &s->ps[pos1]);
+	b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+	b3MprSupportCopy(&s->ps[pos2], &supp);
+}
+
+inline int b3MprIsZero(float val)
+{
+	return B3_MPR_FABS(val) < FLT_EPSILON;
+}
+
+inline int b3MprEq(float _a, float _b)
+{
+	float ab;
+	float a, b;
+
+	ab = B3_MPR_FABS(_a - _b);
+	if (B3_MPR_FABS(ab) < FLT_EPSILON)
+		return 1;
+
+	a = B3_MPR_FABS(_a);
+	b = B3_MPR_FABS(_b);
+	if (b > a)
+	{
+		return ab < FLT_EPSILON * b;
+	}
+	else
+	{
+		return ab < FLT_EPSILON * a;
+	}
+}
+
+inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
+{
+	return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
+}
+
+inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
+{
+	b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
+	float maxDot = -B3_LARGE_FLOAT;
+
+	if (0 < hull->m_numVertices)
+	{
+		const b3Float4 scaled = supportVec;
+		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+		return verticesA[hull->m_vertexOffset + index];
+	}
+
+	return supVec;
+}
+
+B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+								  b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+								  b3ConstArray(b3Collidable_t) cpuCollidables,
+								  b3ConstArray(b3Float4) cpuVertices,
+								  __global b3Float4 *sepAxis,
+								  const b3Float4 *_dir, b3Float4 *outp, int logme)
+{
+	//dir is in worldspace, move to local space
+
+	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
+	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
+
+	b3Float4 dir = b3MakeFloat4((*_dir).x, (*_dir).y, (*_dir).z, 0.f);
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
+
+	//find local support vertex
+	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
+
+	b3Assert(cpuCollidables[colIndex].m_shapeType == SHAPE_CONVEX_HULL);
+	__global const b3ConvexPolyhedronData_t *hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
+
+	b3Float4 pInA;
+	if (logme)
+	{
+		//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+		float maxDot = -B3_LARGE_FLOAT;
+
+		if (0 < hull->m_numVertices)
+		{
+			const b3Float4 scaled = localDir;
+			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+			pInA = cpuVertices[hull->m_vertexOffset + index];
+		}
+	}
+	else
+	{
+		pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
+	}
+
+	//move vertex to world space
+	*outp = b3TransformPoint(pInA, pos, orn);
+}
+
+inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+						 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+						 b3ConstArray(b3Collidable_t) cpuCollidables,
+						 b3ConstArray(b3Float4) cpuVertices,
+						 __global b3Float4 *sepAxis,
+						 const b3Float4 *_dir, b3MprSupport_t *supp)
+{
+	b3Float4 dir;
+	dir = *_dir;
+	b3MprConvexSupport(pairIndex, bodyIndexA, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v1, 0);
+	dir = *_dir * -1.f;
+	b3MprConvexSupport(pairIndex, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v2, 0);
+	supp->v = supp->v1 - supp->v2;
+}
+
+inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
+{
+	center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
+	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
+	center->v = center->v1 - center->v2;
+}
+
+inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
+{
+	(*v).x = x;
+	(*v).y = y;
+	(*v).z = z;
+	(*v).w = 0.f;
+}
+
+inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
+{
+	(*v).x += (*w).x;
+	(*v).y += (*w).y;
+	(*v).z += (*w).z;
+}
+
+inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
+{
+	*v = *w;
+}
+
+inline void b3MprVec3Scale(b3Float4 *d, float k)
+{
+	*d *= k;
+}
+
+inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
+{
+	float dot;
+
+	dot = b3Dot3F4(*a, *b);
+	return dot;
+}
+
+inline float b3MprVec3Len2(const b3Float4 *v)
+{
+	return b3MprVec3Dot(v, v);
+}
+
+inline void b3MprVec3Normalize(b3Float4 *d)
+{
+	float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
+	b3MprVec3Scale(d, k);
+}
+
+inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
+{
+	*d = b3Cross3(*a, *b);
+}
+
+inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
+{
+	*d = *v - *w;
+}
+
+inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
+{
+	b3Float4 v2v1, v3v1;
+
+	b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Cross(dir, &v2v1, &v3v1);
+	b3MprVec3Normalize(dir);
+}
+
+inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
+								  const b3Float4 *dir)
+{
+	float dot;
+	dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
+	return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline int portalReachTolerance(const b3MprSimplex_t *portal,
+								const b3MprSupport_t *v4,
+								const b3Float4 *dir)
+{
+	float dv1, dv2, dv3, dv4;
+	float dot1, dot2, dot3;
+
+	// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+
+	dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
+	dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
+	dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
+	dv4 = b3MprVec3Dot(&v4->v, dir);
+
+	dot1 = dv4 - dv1;
+	dot2 = dv4 - dv2;
+	dot3 = dv4 - dv3;
+
+	dot1 = B3_MPR_FMIN(dot1, dot2);
+	dot1 = B3_MPR_FMIN(dot1, dot3);
+
+	return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
+}
+
+inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
+									const b3MprSupport_t *v4,
+									const b3Float4 *dir)
+{
+	float dot;
+	dot = b3MprVec3Dot(&v4->v, dir);
+	return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline void b3ExpandPortal(b3MprSimplex_t *portal,
+						   const b3MprSupport_t *v4)
+{
+	float dot;
+	b3Float4 v4v0;
+
+	b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
+	if (dot > 0.f)
+	{
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 3, v4);
+		}
+	}
+	else
+	{
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 2, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+	}
+}
+
+B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							   b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							   b3ConstArray(b3Collidable_t) cpuCollidables,
+							   b3ConstArray(b3Float4) cpuVertices,
+							   __global b3Float4 *sepAxis,
+							   __global int *hasSepAxis,
+							   b3MprSimplex_t *portal)
+{
+	b3Float4 dir, va, vb;
+	float dot;
+	int cont;
+
+	// vertex 0 is center of portal
+	b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
+	// vertex 0 is center of portal
+	b3MprSimplexSetSize(portal, 1);
+
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
+	{
+		// Portal's center lies on origin (0,0,0) => we know that objects
+		// intersect but we would need to know penetration info.
+		// So move center little bit...
+		b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+		b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
+	}
+
+	// vertex 1 = support in direction of origin
+	b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Scale(&dir, -1.f);
+	b3MprVec3Normalize(&dir);
+
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
+
+	b3MprSimplexSetSize(portal, 2);
+
+	// test if origin isn't outside of v1
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
+
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	// vertex 2
+	b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	if (b3MprIsZero(b3MprVec3Len2(&dir)))
+	{
+		if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
+		{
+			// origin lies on v1
+			return 1;
+		}
+		else
+		{
+			// origin lies on v0-v1 segment
+			return 2;
+		}
+	}
+
+	b3MprVec3Normalize(&dir);
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 2));
+
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	b3MprSimplexSetSize(portal, 3);
+
+	// vertex 3 direction
+	b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Cross(&dir, &va, &vb);
+	b3MprVec3Normalize(&dir);
+
+	// it is better to form portal faces to be oriented "outside" origin
+	dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	if (dot > 0.f)
+	{
+		b3MprSimplexSwap(portal, 1, 2);
+		b3MprVec3Scale(&dir, -1.f);
+	}
+
+	while (b3MprSimplexSize(portal) < 4)
+	{
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
+
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
+		if (b3MprIsZero(dot) || dot < 0.f)
+			return -1;
+
+		cont = 0;
+
+		// test if origin is outside (v1, v0, v3) - set v2 as v3 and
+		// continue
+		b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+		if (dot < 0.f && !b3MprIsZero(dot))
+		{
+			b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
+			cont = 1;
+		}
+
+		if (!cont)
+		{
+			// test if origin is outside (v3, v0, v2) - set v1 as v3 and
+			// continue
+			b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
+						   &b3MprSimplexPoint(portal, 2)->v);
+			dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+			if (dot < 0.f && !b3MprIsZero(dot))
+			{
+				b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
+				cont = 1;
+			}
+		}
+
+		if (cont)
+		{
+			b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Cross(&dir, &va, &vb);
+			b3MprVec3Normalize(&dir);
+		}
+		else
+		{
+			b3MprSimplexSetSize(portal, 4);
+		}
+	}
+
+	return 0;
+}
+
+B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							 b3ConstArray(b3Collidable_t) cpuCollidables,
+							 b3ConstArray(b3Float4) cpuVertices,
+							 __global b3Float4 *sepAxis,
+							 b3MprSimplex_t *portal)
+{
+	b3Float4 dir;
+	b3MprSupport_t v4;
+
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
+	{
+		// compute direction outside the portal (from v0 throught v1,v2,v3
+		// face)
+		b3PortalDir(portal, &dir);
+
+		// test if origin is inside the portal
+		if (portalEncapsulesOrigin(portal, &dir))
+			return 0;
+
+		// get next support point
+
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
+
+		// test if v4 can expand portal to contain origin and if portal
+		// expanding doesn't reach given tolerance
+		if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
+		{
+			return -1;
+		}
+
+		// v1-v2-v3 triangle must be rearranged to face outside Minkowski
+		// difference (direction from v0).
+		b3ExpandPortal(portal, &v4);
+	}
+
+	return -1;
+}
+
+B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+{
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	b3Float4 dir;
+	size_t i;
+	float b[4], sum, inv;
+	b3Float4 vec, p1, p2;
+
+	b3PortalDir(portal, &dir);
+
+	// use barycentric coordinates of tetrahedron to find origin
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+	if (b3MprIsZero(sum) || sum < 0.f)
+	{
+		b[0] = 0.f;
+
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		b[1] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+					   &b3MprSimplexPoint(portal, 1)->v);
+		b[2] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 2)->v);
+		b[3] = b3MprVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = 1.f / sum;
+
+	b3MprVec3Copy(&p1, b3mpr_vec3_origin);
+	b3MprVec3Copy(&p2, b3mpr_vec3_origin);
+	for (i = 0; i < 4; i++)
+	{
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p1, &vec);
+
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p2, &vec);
+	}
+	b3MprVec3Scale(&p1, inv);
+	b3MprVec3Scale(&p2, inv);
+
+	b3MprVec3Copy(pos, &p1);
+	b3MprVec3Add(pos, &p2);
+	b3MprVec3Scale(pos, 0.5);
+}
+
+inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
+{
+	b3Float4 ab;
+	b3MprVec3Sub2(&ab, a, b);
+	return b3MprVec3Len2(&ab);
+}
+
+inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
+										 const b3Float4 *x0,
+										 const b3Float4 *b,
+										 b3Float4 *witness)
+{
+	// The computation comes from solving equation of segment:
+	//      S(t) = x0 + t.d
+	//          where - x0 is initial point of segment
+	//                - d is direction of segment from x0 (|d| > 0)
+	//                - t belongs to <0, 1> interval
+	//
+	// Than, distance from a segment to some point P can be expressed:
+	//      D(t) = |x0 + t.d - P|^2
+	//          which is distance from any point on segment. Minimization
+	//          of this function brings distance from P to segment.
+	// Minimization of D(t) leads to simple quadratic equation that's
+	// solving is straightforward.
+	//
+	// Bonus of this method is witness point for free.
+
+	float dist, t;
+	b3Float4 d, a;
+
+	// direction of segment
+	b3MprVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	b3MprVec3Sub2(&a, x0, P);
+
+	t = -1.f * b3MprVec3Dot(&a, &d);
+	t /= b3MprVec3Len2(&d);
+
+	if (t < 0.f || b3MprIsZero(t))
+	{
+		dist = b3MprVec3Dist2(x0, P);
+		if (witness)
+			b3MprVec3Copy(witness, x0);
+	}
+	else if (t > 1.f || b3MprEq(t, 1.f))
+	{
+		dist = b3MprVec3Dist2(b, P);
+		if (witness)
+			b3MprVec3Copy(witness, b);
+	}
+	else
+	{
+		if (witness)
+		{
+			b3MprVec3Copy(witness, &d);
+			b3MprVec3Scale(witness, t);
+			b3MprVec3Add(witness, x0);
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			// recycling variables
+			b3MprVec3Scale(&d, t);
+			b3MprVec3Add(&d, &a);
+			dist = b3MprVec3Len2(&d);
+		}
+	}
+
+	return dist;
+}
+
+inline float b3MprVec3PointTriDist2(const b3Float4 *P,
+									const b3Float4 *x0, const b3Float4 *B,
+									const b3Float4 *C,
+									b3Float4 *witness)
+{
+	// Computation comes from analytic expression for triangle (x0, B, C)
+	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+	// Then equation for distance is:
+	//      D(s, t) = | T(s, t) - P |^2
+	// This leads to minimization of quadratic function of two variables.
+	// The solution from is taken only if s is between 0 and 1, t is
+	// between 0 and 1 and t + s < 1, otherwise distance from segment is
+	// computed.
+
+	b3Float4 d1, d2, a;
+	float u, v, w, p, q, r;
+	float s, t, dist, dist2;
+	b3Float4 witness2;
+
+	b3MprVec3Sub2(&d1, B, x0);
+	b3MprVec3Sub2(&d2, C, x0);
+	b3MprVec3Sub2(&a, x0, P);
+
+	u = b3MprVec3Dot(&a, &a);
+	v = b3MprVec3Dot(&d1, &d1);
+	w = b3MprVec3Dot(&d2, &d2);
+	p = b3MprVec3Dot(&a, &d1);
+	q = b3MprVec3Dot(&a, &d2);
+	r = b3MprVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
+	{
+		if (witness)
+		{
+			b3MprVec3Scale(&d1, s);
+			b3MprVec3Scale(&d2, t);
+			b3MprVec3Copy(witness, x0);
+			b3MprVec3Add(witness, &d1);
+			b3MprVec3Add(witness, &d2);
+
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += 2.f * s * t * r;
+			dist += 2.f * s * p;
+			dist += 2.f * t * q;
+			dist += u;
+		}
+	}
+	else
+	{
+		dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							b3MprSimplex_t *portal,
+							float *depth, b3Float4 *pdir, b3Float4 *pos)
+{
+	b3Float4 dir;
+	b3MprSupport_t v4;
+	unsigned long iterations;
+
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	iterations = 1UL;
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
+	{
+		// compute portal direction and obtain next support point
+		b3PortalDir(portal, &dir);
+
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
+
+		// reached tolerance -> find penetration info
+		if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
+		{
+			*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
+			*depth = B3_MPR_SQRT(*depth);
+
+			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
+			{
+				*pdir = dir;
+			}
+			b3MprVec3Normalize(pdir);
+
+			// barycentric coordinates:
+			b3FindPos(portal, pos);
+
+			return;
+		}
+
+		b3ExpandPortal(portal, &v4);
+
+		iterations++;
+	}
+}
+
+B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal, float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+	// Touching contact on portal's v1 - so depth is zero and direction
+	// is unimportant and pos can be guessed
+	*depth = 0.f;
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	b3MprVec3Copy(dir, b3mpr_vec3_origin);
+
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5);
+}
+
+B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
+								   float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+	// Origin lies on v0-v1 segment.
+	// Depth is distance to v1, direction also and position must be
+	// computed
+
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5f);
+
+	b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
+	*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
+	b3MprVec3Normalize(dir);
+}
+
+inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
+							b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							__global int *hasSepAxis,
+							float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
+{
+	b3MprSimplex_t portal;
+
+	//	if (!hasSepAxis[pairIndex])
+	//	return -1;
+
+	hasSepAxis[pairIndex] = 0;
+	int res;
+
+	// Phase 1: Portal discovery
+	res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
+
+	//sepAxis[pairIndex] = *pdir;//or -dir?
+
+	switch (res)
+	{
+		case 0:
+		{
+			// Phase 2: Portal refinement
+
+			res = b3RefinePortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal);
+			if (res < 0)
+				return -1;
+
+			// Phase 3. Penetration info
+			b3FindPenetr(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal, depthOut, dirOut, posOut);
+			hasSepAxis[pairIndex] = 1;
+			sepAxis[pairIndex] = -*dirOut;
+			break;
+		}
+		case 1:
+		{
+			// Touching contact on portal's v1.
+			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
+			break;
+		}
+		case 2:
+		{
+			b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
+			break;
+		}
+		default:
+		{
+			hasSepAxis[pairIndex] = 0;
+			//if (res < 0)
+			//{
+			// Origin isn't inside portal - no collision.
+			return -1;
+			//}
+		}
+	};
+
+	return 0;
+};
+
+#endif  //B3_MPR_PENETRATION_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

@@ -0,0 +1,175 @@
+
+#ifndef B3_NEW_CONTACT_REDUCTION_H
+#define B3_NEW_CONTACT_REDUCTION_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
+
+int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
+{
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
+	{
+		for (int i = 0; i < nPoints; i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+
+	//	sample 4 directions
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross(nearNormal, aVector);
+	b3Float4 v = b3Cross(nearNormal, u);
+	u = b3Normalized(u);
+	v = b3Normalized(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
+}
+
+__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
+										  __global const b3RigidBodyData_t* rigidBodies,
+										  __global const b3Float4* separatingNormals,
+										  __global const int* hasSeparatingAxis,
+										  __global struct b3Contact4Data* globalContactsOut,
+										  __global b3Int4* clippingFaces,
+										  __global b3Float4* worldVertsB2,
+										  volatile __global int* nGlobalContactsOut,
+										  int vertexFaceCapacity,
+										  int contactCapacity,
+										  int numPairs,
+										  int pairIndex)
+{
+	//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+
+	b3Int4 contactIdx;
+	contactIdx = b3MakeInt4(0, 1, 2, 3);
+
+	if (i < numPairs)
+	{
+		if (hasSeparatingAxis[i])
+		{
+			int nPoints = clippingFaces[pairIndex].w;
+
+			if (nPoints > 0)
+			{
+				__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
+				b3Float4 normal = -separatingNormals[i];
+
+				int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
+
+				int dstIdx;
+				dstIdx = b3AtomicInc(nGlobalContactsOut);
+
+				//#if 0
+				b3Assert(dstIdx < contactCapacity);
+				if (dstIdx < contactCapacity)
+				{
+					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
+					c->m_worldNormalOnB = -normal;
+					c->m_restituitionCoeffCmp = (0.f * 0xffff);
+					c->m_frictionCoeffCmp = (0.7f * 0xffff);
+					c->m_batchIdx = pairIndex;
+					int bodyA = pairs[pairIndex].x;
+					int bodyB = pairs[pairIndex].y;
+
+					pairs[pairIndex].w = dstIdx;
+
+					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
+					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
+					c->m_childIndexA = -1;
+					c->m_childIndexB = -1;
+
+					switch (nReducedContacts)
+					{
+						case 4:
+							c->m_worldPosB[3] = pointsIn[contactIdx.w];
+						case 3:
+							c->m_worldPosB[2] = pointsIn[contactIdx.z];
+						case 2:
+							c->m_worldPosB[1] = pointsIn[contactIdx.y];
+						case 1:
+							c->m_worldPosB[0] = pointsIn[contactIdx.x];
+						default:
+						{
+						}
+					};
+
+					GET_NPOINTS(*c) = nReducedContacts;
+				}
+
+				//#endif
+
+			}  //		if (numContactsOut>0)
+		}      //		if (hasSeparatingAxis[i])
+	}          //	if (i<numPairs)
+}
+#endif

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

@@ -0,0 +1,88 @@
+
+
+#ifndef B3_QUANTIZED_BVH_NODE_H
+#define B3_QUANTIZED_BVH_NODE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+#define B3_MAX_NUM_PARTS_IN_BITS 10
+
+///b3QuantizedBvhNodeData 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).
+typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
+
+struct b3QuantizedBvhNodeData
+{
+	//12 bytes
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
+	//4 bytes
+	int m_escapeIndexOrTriangleIndex;
+};
+
+inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	unsigned int x = 0;
+	unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
+	// Get only the lower bits where the triangle index is stored
+	return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
+}
+
+inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
+{
+	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
+}
+
+inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	return -rootNode->m_escapeIndexOrTriangleIndex;
+}
+
+inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
+{
+	b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
+	clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
+
+	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
+	if (isMax)
+	{
+		out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
+		out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
+		out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
+	}
+	else
+	{
+		out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
+	}
+}
+
+inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
+	const unsigned short int* aabbMin1,
+	const unsigned short int* aabbMax1,
+	const unsigned short int* aabbMin2,
+	const unsigned short int* aabbMax2)
+{
+	//int overlap = 1;
+	if (aabbMin1[0] > aabbMax2[0])
+		return 0;
+	if (aabbMax1[0] < aabbMin2[0])
+		return 0;
+	if (aabbMin1[1] > aabbMax2[1])
+		return 0;
+	if (aabbMax1[1] < aabbMin2[1])
+		return 0;
+	if (aabbMin1[2] > aabbMax2[2])
+		return 0;
+	if (aabbMax1[2] < aabbMin2[2])
+		return 0;
+	return 1;
+	//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
+	//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
+	//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
+	//return overlap;
+}
+
+#endif  //B3_QUANTIZED_BVH_NODE_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h

@@ -0,0 +1,89 @@
+#ifndef B3_REDUCE_CONTACTS_H
+#define B3_REDUCE_CONTACTS_H
+
+inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
+{
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
+	{
+		for (int i = 0; i < nPoints; i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+
+	//	sample 4 directions
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross3(nearNormal, aVector);
+	b3Float4 v = b3Cross3(nearNormal, u);
+	u = b3FastNormalized3(u);
+	v = b3FastNormalized3(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot3F4(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot3F4(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot3F4(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot3F4(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
+}
+
+#endif  //B3_REDUCE_CONTACTS_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h

@@ -0,0 +1,31 @@
+#ifndef B3_RIGIDBODY_DATA_H
+#define B3_RIGIDBODY_DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+typedef struct b3RigidBodyData b3RigidBodyData_t;
+
+struct b3RigidBodyData
+{
+	b3Float4 m_pos;
+	b3Quat m_quat;
+	b3Float4 m_linVel;
+	b3Float4 m_angVel;
+
+	int m_collidableIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+};
+
+typedef struct b3InertiaData b3InertiaData_t;
+
+struct b3InertiaData
+{
+	b3Mat3x3 m_invInertiaWorld;
+	b3Mat3x3 m_initInvInertia;
+};
+
+#endif  //B3_RIGIDBODY_DATA_H

Engine/lib/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h

@@ -0,0 +1,35 @@
+#ifndef B3_UPDATE_AABBS_H
+#define B3_UPDATE_AABBS_H
+
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
+{
+	__global const b3RigidBodyData_t* body = &bodies[bodyId];
+
+	b3Float4 position = body->m_pos;
+	b3Quat orientation = body->m_quat;
+
+	int collidableIndex = body->m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+
+	if (shapeIndex >= 0)
+	{
+		b3Aabb_t localAabb = localShapeAABB[collidableIndex];
+		b3Aabb_t worldAabb;
+
+		b3Float4 aabbAMinOut, aabbAMaxOut;
+		float margin = 0.f;
+		b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
+
+		worldAabb.m_minVec = aabbAMinOut;
+		worldAabb.m_minIndices[3] = bodyId;
+		worldAabb.m_maxVec = aabbAMaxOut;
+		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
+		worldAabbs[bodyId] = worldAabb;
+	}
+}
+
+#endif  //B3_UPDATE_AABBS_H

Engine/lib/bullet/src/Bullet3Collision/premake4.lua

@@ -0,0 +1,16 @@
+	project "Bullet3Collision"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

Engine/lib/bullet/src/Bullet3Common/CMakeLists.txt

@@ -0,0 +1,63 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Common_SRCS
+	b3AlignedAllocator.cpp
+	b3Vector3.cpp
+	b3Logging.cpp
+)
+
+SET(Bullet3Common_HDRS
+	b3AlignedAllocator.h
+	b3AlignedObjectArray.h
+	b3CommandLineArgs.h
+	b3HashMap.h
+	b3Logging.h
+	b3Matrix3x3.h
+	b3MinMax.h
+	b3PoolAllocator.h
+	b3QuadWord.h
+	b3Quaternion.h
+	b3Random.h
+	b3Scalar.h
+	b3StackAlloc.h
+	b3Transform.h
+	b3TransformUtil.h
+	b3Vector3.h
+	shared/b3Float4
+	shared/b3Int2.h
+	shared/b3Int4.h
+	shared/b3Mat3x3.h
+	shared/b3PlatformDefinitions
+	shared/b3Quat.h
+)
+
+ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common
+					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)
+			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(Bullet3Common PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}")
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.cpp

@@ -0,0 +1,186 @@
+/*
+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 "b3AlignedAllocator.h"
+
+#ifdef B3_ALLOCATOR_STATISTICS
+int b3g_numAlignedAllocs = 0;
+int b3g_numAlignedFree = 0;
+int b3g_totalBytesAlignedAllocs = 0;  //detect memory leaks
+#endif
+
+static void *b3AllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void b3FreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
+static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
+
+#if defined(B3_HAS_ALIGNED_ALLOCATOR)
+#include <malloc.h>
+static void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void b3AlignedFreeDefault(void *ptr)
+{
+	_aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return memalign(alignment, size);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+#else
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	void *ret;
+	char *real;
+	real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = b3AlignPointer(real + sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+	}
+	else
+	{
+		ret = (void *)(real);
+	}
+	return (ret);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+	void *real;
+
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		b3s_freeFunc(real);
+	}
+}
+#endif
+
+static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
+static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
+
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
+{
+	b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
+	b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
+}
+
+void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
+{
+	b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
+	b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
+}
+
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
+{
+	void *ret;
+	char *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_totalBytesAlignedAllocs += size;
+	b3g_numAlignedAllocs++;
+#endif
+	real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+		*((int *)(ret)-2) = size;
+	}
+	else
+	{
+		ret = (void *)(real);  //??
+	}
+
+	b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
+
+	int *ptr = (int *)ret;
+	*ptr = 12;
+	return (ret);
+}
+
+void b3AlignedFreeInternal(void *ptr, int line, char *filename)
+{
+	void *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedFree++;
+#endif
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		int size = *((int *)(ptr)-2);
+#ifdef B3_ALLOCATOR_STATISTICS
+		b3g_totalBytesAlignedAllocs -= size;
+#endif
+		b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
+
+		b3s_freeFunc(real);
+	}
+	else
+	{
+		b3Printf("NULL ptr\n");
+	}
+}
+
+#else  //B3_DEBUG_MEMORY_ALLOCATIONS
+
+void *b3AlignedAllocInternal(size_t size, int alignment)
+{
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedAllocs++;
+#endif
+	void *ptr;
+	ptr = b3s_alignedAllocFunc(size, alignment);
+	//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
+	return ptr;
+}
+
+void b3AlignedFreeInternal(void *ptr)
+{
+	if (!ptr)
+	{
+		return;
+	}
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedFree++;
+#endif
+	//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
+	b3s_alignedFreeFunc(ptr);
+}
+
+#endif  //B3_DEBUG_MEMORY_ALLOCATIONS

Engine/lib/bullet/src/Bullet3Common/b3AlignedAllocator.h

@@ -0,0 +1,110 @@
+/*
+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 B3_ALIGNED_ALLOCATOR
+#define B3_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "b3Scalar.h"
+//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+
+#define b3AlignedAlloc(a, b) \
+	b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
+
+#define b3AlignedFree(ptr) \
+	b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
+
+void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
+
+void b3AlignedFreeInternal(void* ptr, int line, char* filename);
+
+#else
+void* b3AlignedAllocInternal(size_t size, int alignment);
+void b3AlignedFreeInternal(void* ptr);
+
+#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
+#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
+
+#endif
+typedef int btSizeType;
+
+typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
+typedef void(b3AlignedFreeFunc)(void* memblock);
+typedef void*(b3AllocFunc)(size_t size);
+typedef void(b3FreeFunc)(void* memblock);
+
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
+void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
+///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
+
+///The b3AlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
+template <typename T, unsigned Alignment>
+class b3AlignedAllocator
+{
+	typedef b3AlignedAllocator<T, Alignment> self_type;
+
+public:
+	//just going down a list:
+	b3AlignedAllocator() {}
+	/*
+	b3AlignedAllocator( const self_type & ) {}
+	*/
+
+	template <typename Other>
+	b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
+	{
+	}
+
+	typedef const T* const_pointer;
+	typedef const T& const_reference;
+	typedef T* pointer;
+	typedef T& reference;
+	typedef T value_type;
+
+	pointer address(reference ref) const { return &ref; }
+	const_pointer address(const_reference ref) const { return &ref; }
+	pointer allocate(btSizeType n, const_pointer* hint = 0)
+	{
+		(void)hint;
+		return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
+	}
+	void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+	void deallocate(pointer ptr)
+	{
+		b3AlignedFree(reinterpret_cast<void*>(ptr));
+	}
+	void destroy(pointer ptr) { ptr->~value_type(); }
+
+	template <typename O>
+	struct rebind
+	{
+		typedef b3AlignedAllocator<O, Alignment> other;
+	};
+	template <typename O>
+	self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
+	{
+		return *this;
+	}
+
+	friend bool operator==(const self_type&, const self_type&) { return true; }
+};
+
+#endif  //B3_ALIGNED_ALLOCATOR

Engine/lib/bullet/src/Bullet3Common/b3AlignedObjectArray.h

@@ -0,0 +1,522 @@
+/*
+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 B3_OBJECT_ARRAY__
+#define B3_OBJECT_ARRAY__
+
+#include "b3Scalar.h"  // has definitions like B3_FORCE_INLINE
+#include "b3AlignedAllocator.h"
+
+///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
+///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define B3_USE_PLACEMENT_NEW 1
+//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+#define B3_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
+
+#ifdef B3_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif  //B3_USE_MEMCPY
+
+#ifdef B3_USE_PLACEMENT_NEW
+#include <new>  //for placement new
+#endif          //B3_USE_PLACEMENT_NEW
+
+///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
+template <typename T>
+//template <class T>
+class b3AlignedObjectArray
+{
+	b3AlignedAllocator<T, 16> m_allocator;
+
+	int m_size;
+	int m_capacity;
+	T* m_data;
+	//PCK: added this line
+	bool m_ownsMemory;
+
+#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
+public:
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
+	{
+		copyFromArray(other);
+		return *this;
+	}
+#else   //B3_ALLOW_ARRAY_COPY_OPERATOR
+private:
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
+#endif  //B3_ALLOW_ARRAY_COPY_OPERATOR
+
+protected:
+	B3_FORCE_INLINE int allocSize(int size)
+	{
+		return (size ? size * 2 : 1);
+	}
+	B3_FORCE_INLINE void copy(int start, int end, T* dest) const
+	{
+		int i;
+		for (i = start; i < end; ++i)
+#ifdef B3_USE_PLACEMENT_NEW
+			new (&dest[i]) T(m_data[i]);
+#else
+			dest[i] = m_data[i];
+#endif  //B3_USE_PLACEMENT_NEW
+	}
+
+	B3_FORCE_INLINE void init()
+	{
+		//PCK: added this line
+		m_ownsMemory = true;
+		m_data = 0;
+		m_size = 0;
+		m_capacity = 0;
+	}
+	B3_FORCE_INLINE void destroy(int first, int last)
+	{
+		int i;
+		for (i = first; i < last; i++)
+		{
+			m_data[i].~T();
+		}
+	}
+
+	B3_FORCE_INLINE void* allocate(int size)
+	{
+		if (size)
+			return m_allocator.allocate(size);
+		return 0;
+	}
+
+	B3_FORCE_INLINE void deallocate()
+	{
+		if (m_data)
+		{
+			//PCK: enclosed the deallocation in this block
+			if (m_ownsMemory)
+			{
+				m_allocator.deallocate(m_data);
+			}
+			m_data = 0;
+		}
+	}
+
+public:
+	b3AlignedObjectArray()
+	{
+		init();
+	}
+
+	~b3AlignedObjectArray()
+	{
+		clear();
+	}
+
+	///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
+	b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
+	{
+		init();
+
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
+
+	/// return the number of elements in the array
+	B3_FORCE_INLINE int size() const
+	{
+		return m_size;
+	}
+
+	B3_FORCE_INLINE const T& at(int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE T& at(int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE const T& operator[](int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE T& operator[](int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void clear()
+	{
+		destroy(0, size());
+
+		deallocate();
+
+		init();
+	}
+
+	B3_FORCE_INLINE void pop_back()
+	{
+		b3Assert(m_size > 0);
+		m_size--;
+		m_data[m_size].~T();
+	}
+
+	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void resizeNoInitialize(int newsize)
+	{
+		int curSize = size();
+
+		if (newsize < curSize)
+		{
+		}
+		else
+		{
+			if (newsize > size())
+			{
+				reserve(newsize);
+			}
+			//leave this uninitialized
+		}
+		m_size = newsize;
+	}
+
+	B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
+	{
+		int curSize = size();
+
+		if (newsize < curSize)
+		{
+			for (int i = newsize; i < curSize; i++)
+			{
+				m_data[i].~T();
+			}
+		}
+		else
+		{
+			if (newsize > size())
+			{
+				reserve(newsize);
+			}
+#ifdef B3_USE_PLACEMENT_NEW
+			for (int i = curSize; i < newsize; i++)
+			{
+				new (&m_data[i]) T(fillData);
+			}
+#endif  //B3_USE_PLACEMENT_NEW
+		}
+
+		m_size = newsize;
+	}
+	B3_FORCE_INLINE T& expandNonInitializing()
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
+
+		return m_data[sz];
+	}
+
+	B3_FORCE_INLINE T& expand(const T& fillValue = T())
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
+#ifdef B3_USE_PLACEMENT_NEW
+		new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
+#endif
+
+		return m_data[sz];
+	}
+
+	B3_FORCE_INLINE void push_back(const T& _Val)
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+
+#ifdef B3_USE_PLACEMENT_NEW
+		new (&m_data[m_size]) T(_Val);
+#else
+		m_data[size()] = _Val;
+#endif  //B3_USE_PLACEMENT_NEW
+
+		m_size++;
+	}
+
+	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+	B3_FORCE_INLINE int capacity() const
+	{
+		return m_capacity;
+	}
+
+	B3_FORCE_INLINE void reserve(int _Count)
+	{  // determine new minimum length of allocated storage
+		if (capacity() < _Count)
+		{  // not enough room, reallocate
+			T* s = (T*)allocate(_Count);
+			b3Assert(s);
+			if (s == 0)
+			{
+				b3Error("b3AlignedObjectArray reserve out-of-memory\n");
+				_Count = 0;
+				m_size = 0;
+			}
+			copy(0, size(), s);
+
+			destroy(0, size());
+
+			deallocate();
+
+			//PCK: added this line
+			m_ownsMemory = true;
+
+			m_data = s;
+
+			m_capacity = _Count;
+		}
+	}
+
+	class less
+	{
+	public:
+		bool operator()(const T& a, const T& b)
+		{
+			return (a < b);
+		}
+	};
+
+	template <typename L>
+	void quickSortInternal(const L& CompareFunc, int lo, int hi)
+	{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+		int i = lo, j = hi;
+		T x = m_data[(lo + hi) / 2];
+
+		//  partition
+		do
+		{
+			while (CompareFunc(m_data[i], x))
+				i++;
+			while (CompareFunc(x, m_data[j]))
+				j--;
+			if (i <= j)
+			{
+				swap(i, j);
+				i++;
+				j--;
+			}
+		} while (i <= j);
+
+		//  recursion
+		if (lo < j)
+			quickSortInternal(CompareFunc, lo, j);
+		if (i < hi)
+			quickSortInternal(CompareFunc, i, hi);
+	}
+
+	template <typename L>
+	void quickSort(const L& CompareFunc)
+	{
+		//don't sort 0 or 1 elements
+		if (size() > 1)
+		{
+			quickSortInternal(CompareFunc, 0, size() - 1);
+		}
+	}
+
+	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+	template <typename L>
+	void downHeap(T* pArr, int k, int n, const L& CompareFunc)
+	{
+		/*  PRE: a[k+1..N] is a heap */
+		/* POST:  a[k..N]  is a heap */
+
+		T temp = pArr[k - 1];
+		/* k has child(s) */
+		while (k <= n / 2)
+		{
+			int child = 2 * k;
+
+			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
+			{
+				child++;
+			}
+			/* pick larger child */
+			if (CompareFunc(temp, pArr[child - 1]))
+			{
+				/* move child up */
+				pArr[k - 1] = pArr[child - 1];
+				k = child;
+			}
+			else
+			{
+				break;
+			}
+		}
+		pArr[k - 1] = temp;
+	} /*downHeap*/
+
+	void swap(int index0, int index1)
+	{
+#ifdef B3_USE_MEMCPY
+		char temp[sizeof(T)];
+		memcpy(temp, &m_data[index0], sizeof(T));
+		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+		memcpy(&m_data[index1], temp, sizeof(T));
+#else
+		T temp = m_data[index0];
+		m_data[index0] = m_data[index1];
+		m_data[index1] = temp;
+#endif  //B3_USE_PLACEMENT_NEW
+	}
+
+	template <typename L>
+	void heapSort(const L& CompareFunc)
+	{
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = m_size;
+		for (k = n / 2; k > 0; k--)
+		{
+			downHeap(m_data, k, n, CompareFunc);
+		}
+
+		/* a[1..N] is now a heap */
+		while (n >= 1)
+		{
+			swap(0, n - 1); /* largest of a[0..n-1] */
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(m_data, 1, n, CompareFunc);
+		}
+	}
+
+	///non-recursive binary search, assumes sorted array
+	int findBinarySearch(const T& key) const
+	{
+		int first = 0;
+		int last = size() - 1;
+
+		//assume sorted array
+		while (first <= last)
+		{
+			int mid = (first + last) / 2;  // compute mid point.
+			if (key > m_data[mid])
+				first = mid + 1;  // repeat search in top half.
+			else if (key < m_data[mid])
+				last = mid - 1;  // repeat search in bottom half.
+			else
+				return mid;  // found it. return position /////
+		}
+		return size();  // failed to find key
+	}
+
+	int findLinearSearch(const T& key) const
+	{
+		int index = size();
+		int i;
+
+		for (i = 0; i < size(); i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	int findLinearSearch2(const T& key) const
+	{
+		int index = -1;
+		int i;
+
+		for (i = 0; i < size(); i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void remove(const T& key)
+	{
+		int findIndex = findLinearSearch(key);
+		if (findIndex < size())
+		{
+			swap(findIndex, size() - 1);
+			pop_back();
+		}
+	}
+
+	//PCK: whole function
+	void initializeFromBuffer(void* buffer, int size, int capacity)
+	{
+		clear();
+		m_ownsMemory = false;
+		m_data = (T*)buffer;
+		m_size = size;
+		m_capacity = capacity;
+	}
+
+	void copyFromArray(const b3AlignedObjectArray& otherArray)
+	{
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
+
+	void removeAtIndex(int index)
+	{
+		if (index < size())
+		{
+			swap(index, size() - 1);
+			pop_back();
+		}
+	}
+};
+
+#endif  //B3_OBJECT_ARRAY__

Engine/lib/bullet/src/Bullet3Common/b3CommandLineArgs.h

@@ -0,0 +1,106 @@
+#ifndef COMMAND_LINE_ARGS_H
+#define COMMAND_LINE_ARGS_H
+
+/******************************************************************************
+ * Command-line parsing
+ ******************************************************************************/
+#include <map>
+#include <algorithm>
+#include <string>
+#include <cstring>
+#include <sstream>
+class b3CommandLineArgs
+{
+protected:
+	std::map<std::string, std::string> pairs;
+
+public:
+	// Constructor
+	b3CommandLineArgs(int argc, char **argv)
+	{
+		addArgs(argc, argv);
+	}
+
+	void addArgs(int argc, char **argv)
+	{
+		for (int i = 1; i < argc; i++)
+		{
+			std::string arg = argv[i];
+
+			if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
+			{
+				continue;
+			}
+
+			std::string::size_type pos;
+			std::string key, val;
+			if ((pos = arg.find('=')) == std::string::npos)
+			{
+				key = std::string(arg, 2, arg.length() - 2);
+				val = "";
+			}
+			else
+			{
+				key = std::string(arg, 2, pos - 2);
+				val = std::string(arg, pos + 1, arg.length() - 1);
+			}
+
+			//only add new keys, don't replace existing
+			if (pairs.find(key) == pairs.end())
+			{
+				pairs[key] = val;
+			}
+		}
+	}
+
+	bool CheckCmdLineFlag(const char *arg_name)
+	{
+		std::map<std::string, std::string>::iterator itr;
+		if ((itr = pairs.find(arg_name)) != pairs.end())
+		{
+			return true;
+		}
+		return false;
+	}
+
+	template <typename T>
+	bool GetCmdLineArgument(const char *arg_name, T &val);
+
+	int ParsedArgc()
+	{
+		return pairs.size();
+	}
+};
+
+template <typename T>
+inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
+{
+	std::map<std::string, std::string>::iterator itr;
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
+		std::istringstream strstream(itr->second);
+		strstream >> val;
+		return true;
+	}
+	return false;
+}
+
+template <>
+inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
+{
+	std::map<std::string, std::string>::iterator itr;
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
+		std::string s = itr->second;
+		val = (char *)malloc(sizeof(char) * (s.length() + 1));
+		std::strcpy(val, s.c_str());
+		return true;
+	}
+	else
+	{
+		val = NULL;
+	}
+	return false;
+}
+
+#endif  //COMMAND_LINE_ARGS_H

Engine/lib/bullet/src/Bullet3Common/b3FileUtils.h

@@ -0,0 +1,133 @@
+#ifndef B3_FILE_UTILS_H
+#define B3_FILE_UTILS_H
+
+#include <stdio.h>
+#include "b3Scalar.h"
+#include <stddef.h>  //ptrdiff_h
+#include <string.h>
+
+struct b3FileUtils
+{
+	b3FileUtils()
+	{
+	}
+	virtual ~b3FileUtils()
+	{
+	}
+
+	static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
+	{
+		FILE* f = 0;
+		f = fopen(orgFileName, "rb");
+		if (f)
+		{
+			//printf("original file found: [%s]\n", orgFileName);
+			sprintf(relativeFileName, "%s", orgFileName);
+			fclose(f);
+			return true;
+		}
+
+		//printf("Trying various directories, relative to current working directory\n");
+		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+
+		f = 0;
+		bool fileFound = false;
+
+		for (int i = 0; !f && i < numPrefixes; i++)
+		{
+#ifdef _MSC_VER
+			sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
+#else
+			sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
+#endif
+			f = fopen(relativeFileName, "rb");
+			if (f)
+			{
+				fileFound = true;
+				break;
+			}
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
+		return fileFound;
+	}
+
+	static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char* oriptr;
+		const char* patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+
+	static int extractPath(const char* fileName, char* path, int maxPathLength)
+	{
+		const char* stripped = strip2(fileName, "/");
+		stripped = strip2(stripped, "\\");
+
+		ptrdiff_t len = stripped - fileName;
+		b3Assert((len + 1) < maxPathLength);
+
+		if (len && ((len + 1) < maxPathLength))
+		{
+			for (int i = 0; i < len; i++)
+			{
+				path[i] = fileName[i];
+			}
+			path[len] = 0;
+		}
+		else
+		{
+			len = 0;
+			b3Assert(maxPathLength > 0);
+			if (maxPathLength > 0)
+			{
+				path[len] = 0;
+			}
+		}
+		return len;
+	}
+
+	static char toLowerChar(const char t)
+	{
+		if (t >= (char)'A' && t <= (char)'Z')
+			return t + ((char)'a' - (char)'A');
+		else
+			return t;
+	}
+
+	static void toLower(char* str)
+	{
+		int len = strlen(str);
+		for (int i = 0; i < len; i++)
+		{
+			str[i] = toLowerChar(str[i]);
+		}
+	}
+
+	/*static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char * oriptr;
+		const char * patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+	*/
+};
+#endif  //B3_FILE_UTILS_H

Engine/lib/bullet/src/Bullet3Common/b3HashMap.h

@@ -0,0 +1,462 @@
+/*
+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 B3_HASH_MAP_H
+#define B3_HASH_MAP_H
+
+#include "b3AlignedObjectArray.h"
+
+#include <string>
+
+///very basic hashable string implementation, compatible with b3HashMap
+struct b3HashString
+{
+	std::string m_string;
+	unsigned int m_hash;
+
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		return m_hash;
+	}
+
+	b3HashString(const char* name)
+		: m_string(name)
+	{
+		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
+		static const unsigned int InitialFNV = 2166136261u;
+		static const unsigned int FNVMultiple = 16777619u;
+
+		/* Fowler / Noll / Vo (FNV) Hash */
+		unsigned int hash = InitialFNV;
+		int len = m_string.length();
+		for (int i = 0; i < len; i++)
+		{
+			hash = hash ^ (m_string[i]); /* xor  the low 8 bits */
+			hash = hash * FNVMultiple;   /* multiply by the magic number */
+		}
+		m_hash = hash;
+	}
+
+	int portableStringCompare(const char* src, const char* dst) const
+	{
+		int ret = 0;
+
+		while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
+			++src, ++dst;
+
+		if (ret < 0)
+			ret = -1;
+		else if (ret > 0)
+			ret = 1;
+
+		return (ret);
+	}
+
+	bool equals(const b3HashString& other) const
+	{
+		return (m_string == other.m_string);
+	}
+};
+
+const int B3_HASH_NULL = 0xffffffff;
+
+class b3HashInt
+{
+	int m_uid;
+
+public:
+	b3HashInt(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	void setUid1(int uid)
+	{
+		m_uid = uid;
+	}
+
+	bool equals(const b3HashInt& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+class b3HashPtr
+{
+	union {
+		const void* m_pointer;
+		int m_hashValues[2];
+	};
+
+public:
+	b3HashPtr(const void* ptr)
+		: m_pointer(ptr)
+	{
+	}
+
+	const void* getPointer() const
+	{
+		return m_pointer;
+	}
+
+	bool equals(const b3HashPtr& other) const
+	{
+		return getPointer() == other.getPointer();
+	}
+
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
+
+		int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
+
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+template <class Value>
+class b3HashKeyPtr
+{
+	int m_uid;
+
+public:
+	b3HashKeyPtr(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	bool equals(const b3HashKeyPtr<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+template <class Value>
+class b3HashKey
+{
+	int m_uid;
+
+public:
+	b3HashKey(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	bool equals(const b3HashKey<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+///The b3HashMap template class implements a generic and lightweight hashmap.
+///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
+template <class Key, class Value>
+class b3HashMap
+{
+protected:
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+
+	b3AlignedObjectArray<Value> m_valueArray;
+	b3AlignedObjectArray<Key> m_keyArray;
+
+	void growTables(const Key& /*key*/)
+	{
+		int newCapacity = m_valueArray.capacity();
+
+		if (m_hashTable.size() < newCapacity)
+		{
+			//grow hashtable and next table
+			int curHashtableSize = m_hashTable.size();
+
+			m_hashTable.resize(newCapacity);
+			m_next.resize(newCapacity);
+
+			int i;
+
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_hashTable[i] = B3_HASH_NULL;
+			}
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_next[i] = B3_HASH_NULL;
+			}
+
+			for (i = 0; i < curHashtableSize; i++)
+			{
+				//const Value& value = m_valueArray[i];
+				//const Key& key = m_keyArray[i];
+
+				int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1);  // New hash value with new mask
+				m_next[i] = m_hashTable[hashValue];
+				m_hashTable[hashValue] = i;
+			}
+		}
+	}
+
+public:
+	void insert(const Key& key, const Value& value)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		//replace value if the key is already there
+		int index = findIndex(key);
+		if (index != B3_HASH_NULL)
+		{
+			m_valueArray[index] = value;
+			return;
+		}
+
+		int count = m_valueArray.size();
+		int oldCapacity = m_valueArray.capacity();
+		m_valueArray.push_back(value);
+		m_keyArray.push_back(key);
+
+		int newCapacity = m_valueArray.capacity();
+		if (oldCapacity < newCapacity)
+		{
+			growTables(key);
+			//hash with new capacity
+			hash = key.getHash() & (m_valueArray.capacity() - 1);
+		}
+		m_next[count] = m_hashTable[hash];
+		m_hashTable[hash] = count;
+	}
+
+	void remove(const Key& key)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		int pairIndex = findIndex(key);
+
+		if (pairIndex == B3_HASH_NULL)
+		{
+			return;
+		}
+
+		// Remove the pair from the hash table.
+		int index = m_hashTable[hash];
+		b3Assert(index != B3_HASH_NULL);
+
+		int previous = B3_HASH_NULL;
+		while (index != pairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == pairIndex);
+			m_next[previous] = m_next[pairIndex];
+		}
+		else
+		{
+			m_hashTable[hash] = m_next[pairIndex];
+		}
+
+		// We now move the last pair into spot of the
+		// pair being removed. We need to fix the hash
+		// table indices to support the move.
+
+		int lastPairIndex = m_valueArray.size() - 1;
+
+		// If the removed pair is the last pair, we are done.
+		if (lastPairIndex == pairIndex)
+		{
+			m_valueArray.pop_back();
+			m_keyArray.pop_back();
+			return;
+		}
+
+		// Remove the last pair from the hash table.
+		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
+
+		index = m_hashTable[lastHash];
+		b3Assert(index != B3_HASH_NULL);
+
+		previous = B3_HASH_NULL;
+		while (index != lastPairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == lastPairIndex);
+			m_next[previous] = m_next[lastPairIndex];
+		}
+		else
+		{
+			m_hashTable[lastHash] = m_next[lastPairIndex];
+		}
+
+		// Copy the last pair into the remove pair's spot.
+		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+		m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
+
+		// Insert the last pair into the hash table
+		m_next[pairIndex] = m_hashTable[lastHash];
+		m_hashTable[lastHash] = pairIndex;
+
+		m_valueArray.pop_back();
+		m_keyArray.pop_back();
+	}
+
+	int size() const
+	{
+		return m_valueArray.size();
+	}
+
+	const Value* getAtIndex(int index) const
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Value* getAtIndex(int index)
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Key getKeyAtIndex(int index)
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
+
+	const Key getKeyAtIndex(int index) const
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
+
+	Value* operator[](const Key& key)
+	{
+		return find(key);
+	}
+
+	const Value* find(const Key& key) const
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	Value* find(const Key& key)
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	int findIndex(const Key& key) const
+	{
+		unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		if (hash >= (unsigned int)m_hashTable.size())
+		{
+			return B3_HASH_NULL;
+		}
+
+		int index = m_hashTable[hash];
+		while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
+		{
+			index = m_next[index];
+		}
+		return index;
+	}
+
+	void clear()
+	{
+		m_hashTable.clear();
+		m_next.clear();
+		m_valueArray.clear();
+		m_keyArray.clear();
+	}
+};
+
+#endif  //B3_HASH_MAP_H

Engine/lib/bullet/src/Bullet3Common/b3Logging.cpp

@@ -0,0 +1,145 @@
+/*
+Copyright (c) 2013 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+#include "b3Logging.h"
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#ifdef _WIN32
+#include <windows.h>
+#endif  //_WIN32
+
+void b3PrintfFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
+}
+
+void b3WarningMessageFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
+}
+
+void b3ErrorMessageFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+
+	//is this portable?
+	fflush(stdout);
+}
+
+static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
+static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
+static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
+
+///The developer can route b3Printf output using their own implementation
+void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
+{
+	b3s_printfFunc = printfFunc;
+}
+void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc)
+{
+	b3s_warningMessageFunc = warningMessageFunc;
+}
+void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
+{
+	b3s_errorMessageFunc = errorMessageFunc;
+}
+
+//#define B3_MAX_DEBUG_STRING_LENGTH 2048
+#define B3_MAX_DEBUG_STRING_LENGTH 32768
+
+void b3OutputPrintfVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_printfFunc)(strDebug);
+	va_end(argList);
+}
+void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_warningMessageFunc)(strDebug);
+	va_end(argList);
+}
+void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_errorMessageFunc)(strDebug);
+	va_end(argList);
+}
+
+void b3EnterProfileZoneDefault(const char* name)
+{
+}
+void b3LeaveProfileZoneDefault()
+{
+}
+static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
+static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault;
+void b3EnterProfileZone(const char* name)
+{
+	(b3s_enterFunc)(name);
+}
+void b3LeaveProfileZone()
+{
+	(b3s_leaveFunc)();
+}
+
+void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc)
+{
+	b3s_enterFunc = enterFunc;
+}
+void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
+{
+	b3s_leaveFunc = leaveFunc;
+}
+
+#ifndef _MSC_VER
+#undef vsprintf_s
+#endif

Engine/lib/bullet/src/Bullet3Common/b3Logging.h

@@ -0,0 +1,86 @@
+
+#ifndef B3_LOGGING_H
+#define B3_LOGGING_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
+///You can also customize the message by uncommenting out a different line below
+#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
+	//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
+	//#define b3Printf b3OutputPrintfVarArgsInternal
+	//#define b3Printf(...) printf(__VA_ARGS__)
+	//#define b3Printf(...)
+
+#define b3Warning(...)                                                                    \
+	do                                                                                    \
+	{                                                                                     \
+		b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
+		b3OutputWarningMessageVarArgsInternal(__VA_ARGS__);                               \
+	} while (0)
+#define b3Error(...)                                                                  \
+	do                                                                                \
+	{                                                                                 \
+		b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
+		b3OutputErrorMessageVarArgsInternal(__VA_ARGS__);                             \
+	} while (0)
+
+#ifndef B3_NO_PROFILE
+
+	void b3EnterProfileZone(const char* name);
+	void b3LeaveProfileZone();
+#ifdef __cplusplus
+
+	class b3ProfileZone
+	{
+	public:
+		b3ProfileZone(const char* name)
+		{
+			b3EnterProfileZone(name);
+		}
+
+		~b3ProfileZone()
+		{
+			b3LeaveProfileZone();
+		}
+	};
+
+#define B3_PROFILE(name) b3ProfileZone __profile(name)
+#endif
+
+#else  //B3_NO_PROFILE
+
+#define B3_PROFILE(name)
+#define b3StartProfile(a)
+#define b3StopProfile
+
+#endif  //#ifndef B3_NO_PROFILE
+
+	typedef void(b3PrintfFunc)(const char* msg);
+	typedef void(b3WarningMessageFunc)(const char* msg);
+	typedef void(b3ErrorMessageFunc)(const char* msg);
+	typedef void(b3EnterProfileZoneFunc)(const char* msg);
+	typedef void(b3LeaveProfileZoneFunc)();
+
+	///The developer can route b3Printf output using their own implementation
+	void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
+	void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
+	void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
+
+	///Set custom profile zone functions (zones can be nested)
+	void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
+	void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
+
+	///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
+	void b3OutputPrintfVarArgsInternal(const char* str, ...);
+	void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
+	void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //B3_LOGGING_H

Engine/lib/bullet/src/Bullet3Common/b3Matrix3x3.h

@@ -0,0 +1,1354 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_MATRIX3x3_H
+#define B3_MATRIX3x3_H
+
+#include "b3Vector3.h"
+#include "b3Quaternion.h"
+#include <stdio.h>
+
+#ifdef B3_USE_SSE
+//const __m128 B3_ATTRIBUTE_ALIGNED16(b3v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
+#endif
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
+#endif
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3Matrix3x3Data b3Matrix3x3DoubleData
+#else
+#define b3Matrix3x3Data b3Matrix3x3FloatData
+#endif  //B3_USE_DOUBLE_PRECISION
+
+/**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3.
+* Make sure to only include a pure orthogonal matrix without scaling. */
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Matrix3x3
+{
+	///Data storage for the matrix, each vector is a row of the matrix
+	b3Vector3 m_el[3];
+
+public:
+	/** @brief No initializaion constructor */
+	b3Matrix3x3() {}
+
+	//		explicit b3Matrix3x3(const b3Scalar *m) { setFromOpenGLSubMatrix(m); }
+
+	/**@brief Constructor from Quaternion */
+	explicit b3Matrix3x3(const b3Quaternion& q) { setRotation(q); }
+	/*
+	template <typename b3Scalar>
+	Matrix3x3(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{ 
+	setEulerYPR(yaw, pitch, roll);
+	}
+	*/
+	/** @brief Constructor with row major formatting */
+	b3Matrix3x3(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
+				const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
+				const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
+	{
+		setValue(xx, xy, xz,
+				 yx, yy, yz,
+				 zx, zy, zz);
+	}
+
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	B3_FORCE_INLINE b3Matrix3x3(const b3SimdFloat4 v0, const b3SimdFloat4 v1, const b3SimdFloat4 v2)
+	{
+		m_el[0].mVec128 = v0;
+		m_el[1].mVec128 = v1;
+		m_el[2].mVec128 = v2;
+	}
+
+	B3_FORCE_INLINE b3Matrix3x3(const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2)
+	{
+		m_el[0] = v0;
+		m_el[1] = v1;
+		m_el[2] = v2;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& rhs)
+	{
+		m_el[0].mVec128 = rhs.m_el[0].mVec128;
+		m_el[1].mVec128 = rhs.m_el[1].mVec128;
+		m_el[2].mVec128 = rhs.m_el[2].mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& m)
+	{
+		m_el[0].mVec128 = m.m_el[0].mVec128;
+		m_el[1].mVec128 = m.m_el[1].mVec128;
+		m_el[2].mVec128 = m.m_el[2].mVec128;
+
+		return *this;
+	}
+
+#else
+
+	/** @brief Copy constructor */
+	B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+	}
+
+	/** @brief Assignment Operator */
+	B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+		return *this;
+	}
+
+#endif
+
+	/** @brief Get a column of the matrix as a vector 
+	*  @param i Column number 0 indexed */
+	B3_FORCE_INLINE b3Vector3 getColumn(int i) const
+	{
+		return b3MakeVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
+	}
+
+	/** @brief Get a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	B3_FORCE_INLINE const b3Vector3& getRow(int i) const
+	{
+		b3FullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	/** @brief Get a mutable reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	B3_FORCE_INLINE b3Vector3& operator[](int i)
+	{
+		b3FullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	/** @brief Get a const reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	B3_FORCE_INLINE const b3Vector3& operator[](int i) const
+	{
+		b3FullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	/** @brief Multiply by the target matrix on the right
+	*  @param m Rotation matrix to be applied 
+	* Equivilant to this = this * m */
+	b3Matrix3x3& operator*=(const b3Matrix3x3& m);
+
+	/** @brief Adds by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this + m */
+	b3Matrix3x3& operator+=(const b3Matrix3x3& m);
+
+	/** @brief Substractss by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this - m */
+	b3Matrix3x3& operator-=(const b3Matrix3x3& m);
+
+	/** @brief Set from the rotational part of a 4x4 OpenGL matrix
+	*  @param m A pointer to the beginning of the array of scalars*/
+	void setFromOpenGLSubMatrix(const b3Scalar* m)
+	{
+		m_el[0].setValue(m[0], m[4], m[8]);
+		m_el[1].setValue(m[1], m[5], m[9]);
+		m_el[2].setValue(m[2], m[6], m[10]);
+	}
+	/** @brief Set the values of the matrix explicitly (row major)
+	*  @param xx Top left
+	*  @param xy Top Middle
+	*  @param xz Top Right
+	*  @param yx Middle Left
+	*  @param yy Middle Middle
+	*  @param yz Middle Right
+	*  @param zx Bottom Left
+	*  @param zy Bottom Middle
+	*  @param zz Bottom Right*/
+	void setValue(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
+				  const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
+				  const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
+	{
+		m_el[0].setValue(xx, xy, xz);
+		m_el[1].setValue(yx, yy, yz);
+		m_el[2].setValue(zx, zy, zz);
+	}
+
+	/** @brief Set the matrix from a quaternion
+	*  @param q The Quaternion to match */
+	void setRotation(const b3Quaternion& q)
+	{
+		b3Scalar d = q.length2();
+		b3FullAssert(d != b3Scalar(0.0));
+		b3Scalar s = b3Scalar(2.0) / d;
+
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs, Q = q.get128();
+		__m128i Qi = b3CastfTo128i(Q);
+		__m128 Y, Z;
+		__m128 V1, V2, V3;
+		__m128 V11, V21, V31;
+		__m128 NQ = _mm_xor_ps(Q, b3vMzeroMask);
+		__m128i NQi = b3CastfTo128i(NQ);
+
+		V1 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 2, 3)));  // Y X Z W
+		V2 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(0, 0, 1, 3));                 // -X -X  Y  W
+		V3 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(2, 1, 0, 3)));  // Z Y X W
+		V1 = _mm_xor_ps(V1, b3vMPPP);                                       //	change the sign of the first element
+
+		V11 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 1, 0, 3)));  // Y Y X W
+		V21 = _mm_unpackhi_ps(Q, Q);                                         //  Z  Z  W  W
+		V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(0, 2, 0, 3));                 //  X  Z -X -W
+
+		V2 = V2 * V1;   //
+		V1 = V1 * V11;  //
+		V3 = V3 * V31;  //
+
+		V11 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(2, 3, 1, 3));                //	-Z -W  Y  W
+		V11 = V11 * V21;                                                    //
+		V21 = _mm_xor_ps(V21, b3vMPPP);                                     //	change the sign of the first element
+		V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(3, 3, 1, 3));                //	 W  W -Y -W
+		V31 = _mm_xor_ps(V31, b3vMPPP);                                     //	change the sign of the first element
+		Y = b3CastiTo128f(_mm_shuffle_epi32(NQi, B3_SHUFFLE(3, 2, 0, 3)));  // -W -Z -X -W
+		Z = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 1, 3)));   //  Y  X  Y  W
+
+		vs = _mm_load_ss(&s);
+		V21 = V21 * Y;
+		V31 = V31 * Z;
+
+		V1 = V1 + V11;
+		V2 = V2 + V21;
+		V3 = V3 + V31;
+
+		vs = b3_splat3_ps(vs, 0);
+		//	s ready
+		V1 = V1 * vs;
+		V2 = V2 * vs;
+		V3 = V3 * vs;
+
+		V1 = V1 + b3v1000;
+		V2 = V2 + b3v0100;
+		V3 = V3 + b3v0010;
+
+		m_el[0] = b3MakeVector3(V1);
+		m_el[1] = b3MakeVector3(V2);
+		m_el[2] = b3MakeVector3(V3);
+#else
+		b3Scalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
+		b3Scalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
+		b3Scalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
+		b3Scalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
+		setValue(
+			b3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
+			xy + wz, b3Scalar(1.0) - (xx + zz), yz - wx,
+			xz - wy, yz + wx, b3Scalar(1.0) - (xx + yy));
+#endif
+	}
+
+	/** @brief Set the matrix from euler angles using YPR around YXZ respectively
+	*  @param yaw Yaw about Y axis
+	*  @param pitch Pitch about X axis
+	*  @param roll Roll about Z axis 
+	*/
+	void setEulerYPR(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{
+		setEulerZYX(roll, pitch, yaw);
+	}
+
+	/** @brief Set the matrix from euler angles YPR around ZYX axes
+	* @param eulerX Roll about X axis
+	* @param eulerY Pitch around Y axis
+	* @param eulerZ Yaw aboud Z axis
+	* 
+	* These angles are used to produce a rotation matrix. The euler
+	* angles are applied in ZYX order. I.e a vector is first rotated 
+	* about X then Y and then Z
+	**/
+	void setEulerZYX(b3Scalar eulerX, b3Scalar eulerY, b3Scalar eulerZ)
+	{
+		///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
+		b3Scalar ci(b3Cos(eulerX));
+		b3Scalar cj(b3Cos(eulerY));
+		b3Scalar ch(b3Cos(eulerZ));
+		b3Scalar si(b3Sin(eulerX));
+		b3Scalar sj(b3Sin(eulerY));
+		b3Scalar sh(b3Sin(eulerZ));
+		b3Scalar cc = ci * ch;
+		b3Scalar cs = ci * sh;
+		b3Scalar sc = si * ch;
+		b3Scalar ss = si * sh;
+
+		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+				 cj * sh, sj * ss + cc, sj * cs - sc,
+				 -sj, cj * si, cj * ci);
+	}
+
+	/**@brief Set the matrix to the identity */
+	void setIdentity()
+	{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+		m_el[0] = b3MakeVector3(b3v1000);
+		m_el[1] = b3MakeVector3(b3v0100);
+		m_el[2] = b3MakeVector3(b3v0010);
+#else
+		setValue(b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
+				 b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
+				 b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
+#endif
+	}
+
+	static const b3Matrix3x3& getIdentity()
+	{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+		static const b3Matrix3x3
+			identityMatrix(b3v1000, b3v0100, b3v0010);
+#else
+		static const b3Matrix3x3
+			identityMatrix(
+				b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
+				b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
+				b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
+#endif
+		return identityMatrix;
+	}
+
+	/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
+	* @param m The array to be filled */
+	void getOpenGLSubMatrix(b3Scalar * m) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 v0 = m_el[0].mVec128;
+		__m128 v1 = m_el[1].mVec128;
+		__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
+		__m128* vm = (__m128*)m;
+		__m128 vT;
+
+		v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
+
+		vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
+		v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
+
+		v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
+		v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
+		v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
+
+		vm[0] = v0;
+		vm[1] = v1;
+		vm[2] = v2;
+#elif defined(B3_USE_NEON)
+		// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
+		static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
+		float32x4_t* vm = (float32x4_t*)m;
+		float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
+		float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
+		float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
+		float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
+		float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
+		float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
+
+		vm[0] = v0;
+		vm[1] = v1;
+		vm[2] = v2;
+#else
+		m[0] = b3Scalar(m_el[0].getX());
+		m[1] = b3Scalar(m_el[1].getX());
+		m[2] = b3Scalar(m_el[2].getX());
+		m[3] = b3Scalar(0.0);
+		m[4] = b3Scalar(m_el[0].getY());
+		m[5] = b3Scalar(m_el[1].getY());
+		m[6] = b3Scalar(m_el[2].getY());
+		m[7] = b3Scalar(0.0);
+		m[8] = b3Scalar(m_el[0].getZ());
+		m[9] = b3Scalar(m_el[1].getZ());
+		m[10] = b3Scalar(m_el[2].getZ());
+		m[11] = b3Scalar(0.0);
+#endif
+	}
+
+	/**@brief Get the matrix represented as a quaternion 
+	* @param q The quaternion which will be set */
+	void getRotation(b3Quaternion & q) const
+	{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+		b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
+		b3Scalar s, x;
+
+		union {
+			b3SimdFloat4 vec;
+			b3Scalar f[4];
+		} temp;
+
+		if (trace > b3Scalar(0.0))
+		{
+			x = trace + b3Scalar(1.0);
+
+			temp.f[0] = m_el[2].getY() - m_el[1].getZ();
+			temp.f[1] = m_el[0].getZ() - m_el[2].getX();
+			temp.f[2] = m_el[1].getX() - m_el[0].getY();
+			temp.f[3] = x;
+			//temp.f[3]= s * b3Scalar(0.5);
+		}
+		else
+		{
+			int i, j, k;
+			if (m_el[0].getX() < m_el[1].getY())
+			{
+				if (m_el[1].getY() < m_el[2].getZ())
+				{
+					i = 2;
+					j = 0;
+					k = 1;
+				}
+				else
+				{
+					i = 1;
+					j = 2;
+					k = 0;
+				}
+			}
+			else
+			{
+				if (m_el[0].getX() < m_el[2].getZ())
+				{
+					i = 2;
+					j = 0;
+					k = 1;
+				}
+				else
+				{
+					i = 0;
+					j = 1;
+					k = 2;
+				}
+			}
+
+			x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0);
+
+			temp.f[3] = (m_el[k][j] - m_el[j][k]);
+			temp.f[j] = (m_el[j][i] + m_el[i][j]);
+			temp.f[k] = (m_el[k][i] + m_el[i][k]);
+			temp.f[i] = x;
+			//temp.f[i] = s * b3Scalar(0.5);
+		}
+
+		s = b3Sqrt(x);
+		q.set128(temp.vec);
+		s = b3Scalar(0.5) / s;
+
+		q *= s;
+#else
+		b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
+
+		b3Scalar temp[4];
+
+		if (trace > b3Scalar(0.0))
+		{
+			b3Scalar s = b3Sqrt(trace + b3Scalar(1.0));
+			temp[3] = (s * b3Scalar(0.5));
+			s = b3Scalar(0.5) / s;
+
+			temp[0] = ((m_el[2].getY() - m_el[1].getZ()) * s);
+			temp[1] = ((m_el[0].getZ() - m_el[2].getX()) * s);
+			temp[2] = ((m_el[1].getX() - m_el[0].getY()) * s);
+		}
+		else
+		{
+			int i = m_el[0].getX() < m_el[1].getY() ? (m_el[1].getY() < m_el[2].getZ() ? 2 : 1) : (m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
+			int j = (i + 1) % 3;
+			int k = (i + 2) % 3;
+
+			b3Scalar s = b3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
+			temp[i] = s * b3Scalar(0.5);
+			s = b3Scalar(0.5) / s;
+
+			temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+			temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+			temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+		}
+		q.setValue(temp[0], temp[1], temp[2], temp[3]);
+#endif
+	}
+
+	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+	* @param yaw Yaw around Y axis
+	* @param pitch Pitch around X axis
+	* @param roll around Z axis */
+	void getEulerYPR(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll) const
+	{
+		// first use the normal calculus
+		yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX()));
+		pitch = b3Scalar(b3Asin(-m_el[2].getX()));
+		roll = b3Scalar(b3Atan2(m_el[2].getY(), m_el[2].getZ()));
+
+		// on pitch = +/-HalfPI
+		if (b3Fabs(pitch) == B3_HALF_PI)
+		{
+			if (yaw > 0)
+				yaw -= B3_PI;
+			else
+				yaw += B3_PI;
+
+			if (roll > 0)
+				roll -= B3_PI;
+			else
+				roll += B3_PI;
+		}
+	};
+
+	/**@brief Get the matrix represented as euler angles around ZYX
+	* @param yaw Yaw around X axis
+	* @param pitch Pitch around Y axis
+	* @param roll around X axis 
+	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
+	void getEulerZYX(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll, unsigned int solution_number = 1) const
+	{
+		struct Euler
+		{
+			b3Scalar yaw;
+			b3Scalar pitch;
+			b3Scalar roll;
+		};
+
+		Euler euler_out;
+		Euler euler_out2;  //second solution
+		//get the pointer to the raw data
+
+		// Check that pitch is not at a singularity
+		if (b3Fabs(m_el[2].getX()) >= 1)
+		{
+			euler_out.yaw = 0;
+			euler_out2.yaw = 0;
+
+			// From difference of angles formula
+			b3Scalar delta = b3Atan2(m_el[0].getX(), m_el[0].getZ());
+			if (m_el[2].getX() > 0)  //gimbal locked up
+			{
+				euler_out.pitch = B3_PI / b3Scalar(2.0);
+				euler_out2.pitch = B3_PI / b3Scalar(2.0);
+				euler_out.roll = euler_out.pitch + delta;
+				euler_out2.roll = euler_out.pitch + delta;
+			}
+			else  // gimbal locked down
+			{
+				euler_out.pitch = -B3_PI / b3Scalar(2.0);
+				euler_out2.pitch = -B3_PI / b3Scalar(2.0);
+				euler_out.roll = -euler_out.pitch + delta;
+				euler_out2.roll = -euler_out.pitch + delta;
+			}
+		}
+		else
+		{
+			euler_out.pitch = -b3Asin(m_el[2].getX());
+			euler_out2.pitch = B3_PI - euler_out.pitch;
+
+			euler_out.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out.pitch),
+									 m_el[2].getZ() / b3Cos(euler_out.pitch));
+			euler_out2.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out2.pitch),
+									  m_el[2].getZ() / b3Cos(euler_out2.pitch));
+
+			euler_out.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out.pitch),
+									m_el[0].getX() / b3Cos(euler_out.pitch));
+			euler_out2.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out2.pitch),
+									 m_el[0].getX() / b3Cos(euler_out2.pitch));
+		}
+
+		if (solution_number == 1)
+		{
+			yaw = euler_out.yaw;
+			pitch = euler_out.pitch;
+			roll = euler_out.roll;
+		}
+		else
+		{
+			yaw = euler_out2.yaw;
+			pitch = euler_out2.pitch;
+			roll = euler_out2.roll;
+		}
+	}
+
+	/**@brief Create a scaled copy of the matrix 
+	* @param s Scaling vector The elements of the vector will scale each column */
+
+	b3Matrix3x3 scaled(const b3Vector3& s) const
+	{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+		return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
+#else
+		return b3Matrix3x3(
+			m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(),
+			m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(),
+			m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ());
+#endif
+	}
+
+	/**@brief Return the determinant of the matrix */
+	b3Scalar determinant() const;
+	/**@brief Return the adjoint of the matrix */
+	b3Matrix3x3 adjoint() const;
+	/**@brief Return the matrix with all values non negative */
+	b3Matrix3x3 absolute() const;
+	/**@brief Return the transpose of the matrix */
+	b3Matrix3x3 transpose() const;
+	/**@brief Return the inverse of the matrix */
+	b3Matrix3x3 inverse() const;
+
+	b3Matrix3x3 transposeTimes(const b3Matrix3x3& m) const;
+	b3Matrix3x3 timesTranspose(const b3Matrix3x3& m) const;
+
+	B3_FORCE_INLINE b3Scalar tdotx(const b3Vector3& v) const
+	{
+		return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ();
+	}
+	B3_FORCE_INLINE b3Scalar tdoty(const b3Vector3& v) const
+	{
+		return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ();
+	}
+	B3_FORCE_INLINE b3Scalar tdotz(const b3Vector3& v) const
+	{
+		return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
+	}
+
+	/**@brief diagonalizes this matrix by the Jacobi method.
+	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+	* coordinate system, i.e., old_this = rot * new_this * rot^T. 
+	* @param threshold See iteration
+	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
+	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
+	* 
+	* Note that this matrix is assumed to be symmetric. 
+	*/
+	void diagonalize(b3Matrix3x3 & rot, b3Scalar threshold, int maxSteps)
+	{
+		rot.setIdentity();
+		for (int step = maxSteps; step > 0; step--)
+		{
+			// find off-diagonal element [p][q] with largest magnitude
+			int p = 0;
+			int q = 1;
+			int r = 2;
+			b3Scalar max = b3Fabs(m_el[0][1]);
+			b3Scalar v = b3Fabs(m_el[0][2]);
+			if (v > max)
+			{
+				q = 2;
+				r = 1;
+				max = v;
+			}
+			v = b3Fabs(m_el[1][2]);
+			if (v > max)
+			{
+				p = 1;
+				q = 2;
+				r = 0;
+				max = v;
+			}
+
+			b3Scalar t = threshold * (b3Fabs(m_el[0][0]) + b3Fabs(m_el[1][1]) + b3Fabs(m_el[2][2]));
+			if (max <= t)
+			{
+				if (max <= B3_EPSILON * t)
+				{
+					return;
+				}
+				step = 1;
+			}
+
+			// compute Jacobi rotation J which leads to a zero for element [p][q]
+			b3Scalar mpq = m_el[p][q];
+			b3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+			b3Scalar theta2 = theta * theta;
+			b3Scalar cos;
+			b3Scalar sin;
+			if (theta2 * theta2 < b3Scalar(10 / B3_EPSILON))
+			{
+				t = (theta >= 0) ? 1 / (theta + b3Sqrt(1 + theta2))
+								 : 1 / (theta - b3Sqrt(1 + theta2));
+				cos = 1 / b3Sqrt(1 + t * t);
+				sin = cos * t;
+			}
+			else
+			{
+				// approximation for large theta-value, i.e., a nearly diagonal matrix
+				t = 1 / (theta * (2 + b3Scalar(0.5) / theta2));
+				cos = 1 - b3Scalar(0.5) * t * t;
+				sin = cos * t;
+			}
+
+			// apply rotation to matrix (this = J^T * this * J)
+			m_el[p][q] = m_el[q][p] = 0;
+			m_el[p][p] -= t * mpq;
+			m_el[q][q] += t * mpq;
+			b3Scalar mrp = m_el[r][p];
+			b3Scalar mrq = m_el[r][q];
+			m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+			m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+			// apply rotation to rot (rot = rot * J)
+			for (int i = 0; i < 3; i++)
+			{
+				b3Vector3& row = rot[i];
+				mrp = row[p];
+				mrq = row[q];
+				row[p] = cos * mrp - sin * mrq;
+				row[q] = cos * mrq + sin * mrp;
+			}
+		}
+	}
+
+	/**@brief Calculate the matrix cofactor 
+	* @param r1 The first row to use for calculating the cofactor
+	* @param c1 The first column to use for calculating the cofactor
+	* @param r1 The second row to use for calculating the cofactor
+	* @param c1 The second column to use for calculating the cofactor
+	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+	*/
+	b3Scalar cofac(int r1, int c1, int r2, int c2) const
+	{
+		return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+	}
+
+	void serialize(struct b3Matrix3x3Data & dataOut) const;
+
+	void serializeFloat(struct b3Matrix3x3FloatData & dataOut) const;
+
+	void deSerialize(const struct b3Matrix3x3Data& dataIn);
+
+	void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn);
+
+	void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn);
+};
+
+B3_FORCE_INLINE b3Matrix3x3&
+b3Matrix3x3::operator*=(const b3Matrix3x3& m)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 rv00, rv01, rv02;
+	__m128 rv10, rv11, rv12;
+	__m128 rv20, rv21, rv22;
+	__m128 mv0, mv1, mv2;
+
+	rv02 = m_el[0].mVec128;
+	rv12 = m_el[1].mVec128;
+	rv22 = m_el[2].mVec128;
+
+	mv0 = _mm_and_ps(m[0].mVec128, b3vFFF0fMask);
+	mv1 = _mm_and_ps(m[1].mVec128, b3vFFF0fMask);
+	mv2 = _mm_and_ps(m[2].mVec128, b3vFFF0fMask);
+
+	// rv0
+	rv00 = b3_splat_ps(rv02, 0);
+	rv01 = b3_splat_ps(rv02, 1);
+	rv02 = b3_splat_ps(rv02, 2);
+
+	rv00 = _mm_mul_ps(rv00, mv0);
+	rv01 = _mm_mul_ps(rv01, mv1);
+	rv02 = _mm_mul_ps(rv02, mv2);
+
+	// rv1
+	rv10 = b3_splat_ps(rv12, 0);
+	rv11 = b3_splat_ps(rv12, 1);
+	rv12 = b3_splat_ps(rv12, 2);
+
+	rv10 = _mm_mul_ps(rv10, mv0);
+	rv11 = _mm_mul_ps(rv11, mv1);
+	rv12 = _mm_mul_ps(rv12, mv2);
+
+	// rv2
+	rv20 = b3_splat_ps(rv22, 0);
+	rv21 = b3_splat_ps(rv22, 1);
+	rv22 = b3_splat_ps(rv22, 2);
+
+	rv20 = _mm_mul_ps(rv20, mv0);
+	rv21 = _mm_mul_ps(rv21, mv1);
+	rv22 = _mm_mul_ps(rv22, mv2);
+
+	rv00 = _mm_add_ps(rv00, rv01);
+	rv10 = _mm_add_ps(rv10, rv11);
+	rv20 = _mm_add_ps(rv20, rv21);
+
+	m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
+	m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
+	m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t rv0, rv1, rv2;
+	float32x4_t v0, v1, v2;
+	float32x4_t mv0, mv1, mv2;
+
+	v0 = m_el[0].mVec128;
+	v1 = m_el[1].mVec128;
+	v2 = m_el[2].mVec128;
+
+	mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
+	mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
+	mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
+
+	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
+	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
+	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
+
+	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
+	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
+	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
+
+	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
+	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
+	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
+
+	m_el[0].mVec128 = rv0;
+	m_el[1].mVec128 = rv1;
+	m_el[2].mVec128 = rv2;
+#else
+	setValue(
+		m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+		m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+		m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+#endif
+	return *this;
+}
+
+B3_FORCE_INLINE b3Matrix3x3&
+b3Matrix3x3::operator+=(const b3Matrix3x3& m)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
+	m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
+	m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
+#else
+	setValue(
+		m_el[0][0] + m.m_el[0][0],
+		m_el[0][1] + m.m_el[0][1],
+		m_el[0][2] + m.m_el[0][2],
+		m_el[1][0] + m.m_el[1][0],
+		m_el[1][1] + m.m_el[1][1],
+		m_el[1][2] + m.m_el[1][2],
+		m_el[2][0] + m.m_el[2][0],
+		m_el[2][1] + m.m_el[2][1],
+		m_el[2][2] + m.m_el[2][2]);
+#endif
+	return *this;
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+operator*(const b3Matrix3x3& m, const b3Scalar& k)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	__m128 vk = b3_splat_ps(_mm_load_ss((float*)&k), 0x80);
+	return b3Matrix3x3(
+		_mm_mul_ps(m[0].mVec128, vk),
+		_mm_mul_ps(m[1].mVec128, vk),
+		_mm_mul_ps(m[2].mVec128, vk));
+#elif defined(B3_USE_NEON)
+	return b3Matrix3x3(
+		vmulq_n_f32(m[0].mVec128, k),
+		vmulq_n_f32(m[1].mVec128, k),
+		vmulq_n_f32(m[2].mVec128, k));
+#else
+	return b3Matrix3x3(
+		m[0].getX() * k, m[0].getY() * k, m[0].getZ() * k,
+		m[1].getX() * k, m[1].getY() * k, m[1].getZ() * k,
+		m[2].getX() * k, m[2].getY() * k, m[2].getZ() * k);
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	return b3Matrix3x3(
+		m1[0].mVec128 + m2[0].mVec128,
+		m1[1].mVec128 + m2[1].mVec128,
+		m1[2].mVec128 + m2[2].mVec128);
+#else
+	return b3Matrix3x3(
+		m1[0][0] + m2[0][0],
+		m1[0][1] + m2[0][1],
+		m1[0][2] + m2[0][2],
+
+		m1[1][0] + m2[1][0],
+		m1[1][1] + m2[1][1],
+		m1[1][2] + m2[1][2],
+
+		m1[2][0] + m2[2][0],
+		m1[2][1] + m2[2][1],
+		m1[2][2] + m2[2][2]);
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	return b3Matrix3x3(
+		m1[0].mVec128 - m2[0].mVec128,
+		m1[1].mVec128 - m2[1].mVec128,
+		m1[2].mVec128 - m2[2].mVec128);
+#else
+	return b3Matrix3x3(
+		m1[0][0] - m2[0][0],
+		m1[0][1] - m2[0][1],
+		m1[0][2] - m2[0][2],
+
+		m1[1][0] - m2[1][0],
+		m1[1][1] - m2[1][1],
+		m1[1][2] - m2[1][2],
+
+		m1[2][0] - m2[2][0],
+		m1[2][1] - m2[2][1],
+		m1[2][2] - m2[2][2]);
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3&
+b3Matrix3x3::operator-=(const b3Matrix3x3& m)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
+	m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
+	m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
+#else
+	setValue(
+		m_el[0][0] - m.m_el[0][0],
+		m_el[0][1] - m.m_el[0][1],
+		m_el[0][2] - m.m_el[0][2],
+		m_el[1][0] - m.m_el[1][0],
+		m_el[1][1] - m.m_el[1][1],
+		m_el[1][2] - m.m_el[1][2],
+		m_el[2][0] - m.m_el[2][0],
+		m_el[2][1] - m.m_el[2][1],
+		m_el[2][2] - m.m_el[2][2]);
+#endif
+	return *this;
+}
+
+B3_FORCE_INLINE b3Scalar
+b3Matrix3x3::determinant() const
+{
+	return b3Triple((*this)[0], (*this)[1], (*this)[2]);
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::absolute() const
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	return b3Matrix3x3(
+		_mm_and_ps(m_el[0].mVec128, b3vAbsfMask),
+		_mm_and_ps(m_el[1].mVec128, b3vAbsfMask),
+		_mm_and_ps(m_el[2].mVec128, b3vAbsfMask));
+#elif defined(B3_USE_NEON)
+	return b3Matrix3x3(
+		(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, b3v3AbsMask),
+		(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, b3v3AbsMask),
+		(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, b3v3AbsMask));
+#else
+	return b3Matrix3x3(
+		b3Fabs(m_el[0].getX()), b3Fabs(m_el[0].getY()), b3Fabs(m_el[0].getZ()),
+		b3Fabs(m_el[1].getX()), b3Fabs(m_el[1].getY()), b3Fabs(m_el[1].getZ()),
+		b3Fabs(m_el[2].getX()), b3Fabs(m_el[2].getY()), b3Fabs(m_el[2].getZ()));
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::transpose() const
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	__m128 v0 = m_el[0].mVec128;
+	__m128 v1 = m_el[1].mVec128;
+	__m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
+	__m128 vT;
+
+	v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
+
+	vT = _mm_unpackhi_ps(v0, v1);  //	z0 z1 * *
+	v0 = _mm_unpacklo_ps(v0, v1);  //	x0 x1 y0 y1
+
+	v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
+	v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
+	v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
+
+	return b3Matrix3x3(v0, v1, v2);
+#elif defined(B3_USE_NEON)
+	// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
+	static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
+	float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
+	float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
+	float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
+	float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
+	float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
+	float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
+	return b3Matrix3x3(v0, v1, v2);
+#else
+	return b3Matrix3x3(m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
+					   m_el[0].getY(), m_el[1].getY(), m_el[2].getY(),
+					   m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::adjoint() const
+{
+	return b3Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+					   cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+					   cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::inverse() const
+{
+	b3Vector3 co = b3MakeVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+	b3Scalar det = (*this)[0].dot(co);
+	b3FullAssert(det != b3Scalar(0.0));
+	b3Scalar s = b3Scalar(1.0) / det;
+	return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+					   co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+					   co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	// zeros w
+	//    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
+	__m128 row = m_el[0].mVec128;
+	__m128 m0 = _mm_and_ps(m.getRow(0).mVec128, b3vFFF0fMask);
+	__m128 m1 = _mm_and_ps(m.getRow(1).mVec128, b3vFFF0fMask);
+	__m128 m2 = _mm_and_ps(m.getRow(2).mVec128, b3vFFF0fMask);
+	__m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
+	__m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
+	__m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
+	row = m_el[1].mVec128;
+	r0 = _mm_add_ps(r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
+	row = m_el[2].mVec128;
+	r0 = _mm_add_ps(r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
+	return b3Matrix3x3(r0, r1, r2);
+
+#elif defined B3_USE_NEON
+	// zeros w
+	static const uint32x4_t xyzMask = (const uint32x4_t){-1, -1, -1, 0};
+	float32x4_t m0 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(0).mVec128, xyzMask);
+	float32x4_t m1 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(1).mVec128, xyzMask);
+	float32x4_t m2 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(2).mVec128, xyzMask);
+	float32x4_t row = m_el[0].mVec128;
+	float32x4_t r0 = vmulq_lane_f32(m0, vget_low_f32(row), 0);
+	float32x4_t r1 = vmulq_lane_f32(m0, vget_low_f32(row), 1);
+	float32x4_t r2 = vmulq_lane_f32(m0, vget_high_f32(row), 0);
+	row = m_el[1].mVec128;
+	r0 = vmlaq_lane_f32(r0, m1, vget_low_f32(row), 0);
+	r1 = vmlaq_lane_f32(r1, m1, vget_low_f32(row), 1);
+	r2 = vmlaq_lane_f32(r2, m1, vget_high_f32(row), 0);
+	row = m_el[2].mVec128;
+	r0 = vmlaq_lane_f32(r0, m2, vget_low_f32(row), 0);
+	r1 = vmlaq_lane_f32(r1, m2, vget_low_f32(row), 1);
+	r2 = vmlaq_lane_f32(r2, m2, vget_high_f32(row), 0);
+	return b3Matrix3x3(r0, r1, r2);
+#else
+	return b3Matrix3x3(
+		m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(),
+		m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(),
+		m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(),
+		m_el[0].getY() * m[0].getX() + m_el[1].getY() * m[1].getX() + m_el[2].getY() * m[2].getX(),
+		m_el[0].getY() * m[0].getY() + m_el[1].getY() * m[1].getY() + m_el[2].getY() * m[2].getY(),
+		m_el[0].getY() * m[0].getZ() + m_el[1].getY() * m[1].getZ() + m_el[2].getY() * m[2].getZ(),
+		m_el[0].getZ() * m[0].getX() + m_el[1].getZ() * m[1].getX() + m_el[2].getZ() * m[2].getX(),
+		m_el[0].getZ() * m[0].getY() + m_el[1].getZ() * m[1].getY() + m_el[2].getZ() * m[2].getY(),
+		m_el[0].getZ() * m[0].getZ() + m_el[1].getZ() * m[1].getZ() + m_el[2].getZ() * m[2].getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	__m128 a0 = m_el[0].mVec128;
+	__m128 a1 = m_el[1].mVec128;
+	__m128 a2 = m_el[2].mVec128;
+
+	b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
+	__m128 mx = mT[0].mVec128;
+	__m128 my = mT[1].mVec128;
+	__m128 mz = mT[2].mVec128;
+
+	__m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
+	__m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
+	__m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
+	r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
+	r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
+	r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
+	r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
+	return b3Matrix3x3(r0, r1, r2);
+
+#elif defined B3_USE_NEON
+	float32x4_t a0 = m_el[0].mVec128;
+	float32x4_t a1 = m_el[1].mVec128;
+	float32x4_t a2 = m_el[2].mVec128;
+
+	b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
+	float32x4_t mx = mT[0].mVec128;
+	float32x4_t my = mT[1].mVec128;
+	float32x4_t mz = mT[2].mVec128;
+
+	float32x4_t r0 = vmulq_lane_f32(mx, vget_low_f32(a0), 0);
+	float32x4_t r1 = vmulq_lane_f32(mx, vget_low_f32(a1), 0);
+	float32x4_t r2 = vmulq_lane_f32(mx, vget_low_f32(a2), 0);
+	r0 = vmlaq_lane_f32(r0, my, vget_low_f32(a0), 1);
+	r1 = vmlaq_lane_f32(r1, my, vget_low_f32(a1), 1);
+	r2 = vmlaq_lane_f32(r2, my, vget_low_f32(a2), 1);
+	r0 = vmlaq_lane_f32(r0, mz, vget_high_f32(a0), 0);
+	r1 = vmlaq_lane_f32(r1, mz, vget_high_f32(a1), 0);
+	r2 = vmlaq_lane_f32(r2, mz, vget_high_f32(a2), 0);
+	return b3Matrix3x3(r0, r1, r2);
+
+#else
+	return b3Matrix3x3(
+		m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+		m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+		m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+#endif
+}
+
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Matrix3x3& m, const b3Vector3& v)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	return v.dot3(m[0], m[1], m[2]);
+#else
+	return b3MakeVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+#endif
+}
+
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Vector3& v, const b3Matrix3x3& m)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+
+	const __m128 vv = v.mVec128;
+
+	__m128 c0 = b3_splat_ps(vv, 0);
+	__m128 c1 = b3_splat_ps(vv, 1);
+	__m128 c2 = b3_splat_ps(vv, 2);
+
+	c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, b3vFFF0fMask));
+	c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, b3vFFF0fMask));
+	c0 = _mm_add_ps(c0, c1);
+	c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, b3vFFF0fMask));
+
+	return b3MakeVector3(_mm_add_ps(c0, c2));
+#elif defined(B3_USE_NEON)
+	const float32x4_t vv = v.mVec128;
+	const float32x2_t vlo = vget_low_f32(vv);
+	const float32x2_t vhi = vget_high_f32(vv);
+
+	float32x4_t c0, c1, c2;
+
+	c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
+	c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
+	c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
+
+	c0 = vmulq_lane_f32(c0, vlo, 0);
+	c1 = vmulq_lane_f32(c1, vlo, 1);
+	c2 = vmulq_lane_f32(c2, vhi, 0);
+	c0 = vaddq_f32(c0, c1);
+	c0 = vaddq_f32(c0, c2);
+
+	return b3MakeVector3(c0);
+#else
+	return b3MakeVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+#endif
+}
+
+B3_FORCE_INLINE b3Matrix3x3
+operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+
+	__m128 m10 = m1[0].mVec128;
+	__m128 m11 = m1[1].mVec128;
+	__m128 m12 = m1[2].mVec128;
+
+	__m128 m2v = _mm_and_ps(m2[0].mVec128, b3vFFF0fMask);
+
+	__m128 c0 = b3_splat_ps(m10, 0);
+	__m128 c1 = b3_splat_ps(m11, 0);
+	__m128 c2 = b3_splat_ps(m12, 0);
+
+	c0 = _mm_mul_ps(c0, m2v);
+	c1 = _mm_mul_ps(c1, m2v);
+	c2 = _mm_mul_ps(c2, m2v);
+
+	m2v = _mm_and_ps(m2[1].mVec128, b3vFFF0fMask);
+
+	__m128 c0_1 = b3_splat_ps(m10, 1);
+	__m128 c1_1 = b3_splat_ps(m11, 1);
+	__m128 c2_1 = b3_splat_ps(m12, 1);
+
+	c0_1 = _mm_mul_ps(c0_1, m2v);
+	c1_1 = _mm_mul_ps(c1_1, m2v);
+	c2_1 = _mm_mul_ps(c2_1, m2v);
+
+	m2v = _mm_and_ps(m2[2].mVec128, b3vFFF0fMask);
+
+	c0 = _mm_add_ps(c0, c0_1);
+	c1 = _mm_add_ps(c1, c1_1);
+	c2 = _mm_add_ps(c2, c2_1);
+
+	m10 = b3_splat_ps(m10, 2);
+	m11 = b3_splat_ps(m11, 2);
+	m12 = b3_splat_ps(m12, 2);
+
+	m10 = _mm_mul_ps(m10, m2v);
+	m11 = _mm_mul_ps(m11, m2v);
+	m12 = _mm_mul_ps(m12, m2v);
+
+	c0 = _mm_add_ps(c0, m10);
+	c1 = _mm_add_ps(c1, m11);
+	c2 = _mm_add_ps(c2, m12);
+
+	return b3Matrix3x3(c0, c1, c2);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t rv0, rv1, rv2;
+	float32x4_t v0, v1, v2;
+	float32x4_t mv0, mv1, mv2;
+
+	v0 = m1[0].mVec128;
+	v1 = m1[1].mVec128;
+	v2 = m1[2].mVec128;
+
+	mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, b3vFFF0Mask);
+	mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, b3vFFF0Mask);
+	mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, b3vFFF0Mask);
+
+	rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
+	rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
+	rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
+
+	rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
+	rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
+	rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
+
+	rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
+	rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
+	rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
+
+	return b3Matrix3x3(rv0, rv1, rv2);
+
+#else
+	return b3Matrix3x3(
+		m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
+		m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
+		m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
+#endif
+}
+
+/*
+B3_FORCE_INLINE b3Matrix3x3 b3MultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
+return b3Matrix3x3(
+m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+
+/**@brief Equality operator between two matrices
+* It will test all elements are equal.  */
+B3_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+
+	__m128 c0, c1, c2;
+
+	c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
+	c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
+	c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
+
+	c0 = _mm_and_ps(c0, c1);
+	c0 = _mm_and_ps(c0, c2);
+
+	return (0x7 == _mm_movemask_ps((__m128)c0));
+#else
+	return (m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+			m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+			m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2]);
+#endif
+}
+
+///for serialization
+struct b3Matrix3x3FloatData
+{
+	b3Vector3FloatData m_el[3];
+};
+
+///for serialization
+struct b3Matrix3x3DoubleData
+{
+	b3Vector3DoubleData m_el[3];
+};
+
+B3_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const
+{
+	for (int i = 0; i < 3; i++)
+		m_el[i].serialize(dataOut.m_el[i]);
+}
+
+B3_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& dataOut) const
+{
+	for (int i = 0; i < 3; i++)
+		m_el[i].serializeFloat(dataOut.m_el[i]);
+}
+
+B3_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn)
+{
+	for (int i = 0; i < 3; i++)
+		m_el[i].deSerialize(dataIn.m_el[i]);
+}
+
+B3_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn)
+{
+	for (int i = 0; i < 3; i++)
+		m_el[i].deSerializeFloat(dataIn.m_el[i]);
+}
+
+B3_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn)
+{
+	for (int i = 0; i < 3; i++)
+		m_el[i].deSerializeDouble(dataIn.m_el[i]);
+}
+
+#endif  //B3_MATRIX3x3_H

Engine/lib/bullet/src/Bullet3Common/b3MinMax.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_GEN_MINMAX_H
+#define B3_GEN_MINMAX_H
+
+#include "b3Scalar.h"
+
+template <class T>
+B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
+{
+	return a < b ? a : b;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
+{
+	return a > b ? a : b;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
+{
+	return a < lb ? lb : (ub < a ? ub : a);
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
+{
+	if (b < a)
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
+{
+	if (a < b)
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
+{
+	if (a < lb)
+	{
+		a = lb;
+	}
+	else if (ub < a)
+	{
+		a = ub;
+	}
+}
+
+#endif  //B3_GEN_MINMAX_H

Engine/lib/bullet/src/Bullet3Common/b3PoolAllocator.h

@@ -0,0 +1,121 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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_POOL_ALLOCATOR_H
+#define _BT_POOL_ALLOCATOR_H
+
+#include "b3Scalar.h"
+#include "b3AlignedAllocator.h"
+
+///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
+class b3PoolAllocator
+{
+	int m_elemSize;
+	int m_maxElements;
+	int m_freeCount;
+	void* m_firstFree;
+	unsigned char* m_pool;
+
+public:
+	b3PoolAllocator(int elemSize, int maxElements)
+		: m_elemSize(elemSize),
+		  m_maxElements(maxElements)
+	{
+		m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
+
+		unsigned char* p = m_pool;
+		m_firstFree = p;
+		m_freeCount = m_maxElements;
+		int count = m_maxElements;
+		while (--count)
+		{
+			*(void**)p = (p + m_elemSize);
+			p += m_elemSize;
+		}
+		*(void**)p = 0;
+	}
+
+	~b3PoolAllocator()
+	{
+		b3AlignedFree(m_pool);
+	}
+
+	int getFreeCount() const
+	{
+		return m_freeCount;
+	}
+
+	int getUsedCount() const
+	{
+		return m_maxElements - m_freeCount;
+	}
+
+	int getMaxCount() const
+	{
+		return m_maxElements;
+	}
+
+	void* allocate(int size)
+	{
+		// release mode fix
+		(void)size;
+		b3Assert(!size || size <= m_elemSize);
+		b3Assert(m_freeCount > 0);
+		void* result = m_firstFree;
+		m_firstFree = *(void**)m_firstFree;
+		--m_freeCount;
+		return result;
+	}
+
+	bool validPtr(void* ptr)
+	{
+		if (ptr)
+		{
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	void freeMemory(void* ptr)
+	{
+		if (ptr)
+		{
+			b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+			*(void**)ptr = m_firstFree;
+			m_firstFree = ptr;
+			++m_freeCount;
+		}
+	}
+
+	int getElementSize() const
+	{
+		return m_elemSize;
+	}
+
+	unsigned char* getPoolAddress()
+	{
+		return m_pool;
+	}
+
+	const unsigned char* getPoolAddress() const
+	{
+		return m_pool;
+	}
+};
+
+#endif  //_BT_POOL_ALLOCATOR_H

Engine/lib/bullet/src/Bullet3Common/b3QuadWord.h

@@ -0,0 +1,242 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_SIMD_QUADWORD_H
+#define B3_SIMD_QUADWORD_H
+
+#include "b3Scalar.h"
+#include "b3MinMax.h"
+
+#if defined(__CELLOS_LV2) && defined(__SPU__)
+#include <altivec.h>
+#endif
+
+/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion. 
+ * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
+ */
+#ifndef USE_LIBSPE2
+B3_ATTRIBUTE_ALIGNED16(class)
+b3QuadWord
+#else
+class b3QuadWord
+#endif
+{
+protected:
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		b3Scalar m_floats[4];
+	};
+
+public:
+	vec_float4 get128() const
+	{
+		return mVec128;
+	}
+
+#else  //__CELLOS_LV2__ __SPU__
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+public:
+	union {
+		b3SimdFloat4 mVec128;
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
+	};
+
+public:
+	B3_FORCE_INLINE b3SimdFloat4 get128() const
+	{
+		return mVec128;
+	}
+	B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+public:
+	union {
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
+	};
+#endif  // B3_USE_SSE
+
+#endif  //__CELLOS_LV2__ __SPU__
+
+public:
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+	// Set Vector
+	B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
+	{
+		mVec128 = vec;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
+	{
+		mVec128 = rhs.mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3QuadWord&
+	operator=(const b3QuadWord& v)
+	{
+		mVec128 = v.mVec128;
+
+		return *this;
+	}
+
+#endif
+
+	/**@brief Return the x value */
+	B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+	/**@brief Set the x value */
+	B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
+	/**@brief Return the x value */
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
+	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
+	B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
+
+	B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
+	{
+#ifdef B3_USE_SSE
+		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3] == other.m_floats[3]) &&
+				(m_floats[2] == other.m_floats[2]) &&
+				(m_floats[1] == other.m_floats[1]) &&
+				(m_floats[0] == other.m_floats[0]));
+#endif
+	}
+
+	B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
+	{
+		return !(*this == other);
+	}
+
+	/**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = 0.f;
+	}
+
+	/*		void getValue(b3Scalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] = m_floats[2];
+		}
+*/
+	/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
+	/**@brief No initialization constructor */
+	B3_FORCE_INLINE b3QuadWord()
+	//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
+	{
+	}
+
+	/**@brief Three argument constructor (zeros w)
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
+	}
+
+	/**@brief Initializing constructor
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
+	}
+
+	/**@brief Set each element to the max of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+	B3_FORCE_INLINE void setMax(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		b3SetMax(m_floats[0], other.m_floats[0]);
+		b3SetMax(m_floats[1], other.m_floats[1]);
+		b3SetMax(m_floats[2], other.m_floats[2]);
+		b3SetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+	/**@brief Set each element to the min of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+	B3_FORCE_INLINE void setMin(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		b3SetMin(m_floats[0], other.m_floats[0]);
+		b3SetMin(m_floats[1], other.m_floats[1]);
+		b3SetMin(m_floats[2], other.m_floats[2]);
+		b3SetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
+};
+
+#endif  //B3_SIMD_QUADWORD_H

Engine/lib/bullet/src/Bullet3Common/b3Quaternion.h

@@ -0,0 +1,908 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_SIMD__QUATERNION_H_
+#define B3_SIMD__QUATERNION_H_
+
+#include "b3Vector3.h"
+#include "b3QuadWord.h"
+
+#ifdef B3_USE_SSE
+
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
+
+#endif
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
+
+#endif
+
+/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
+class b3Quaternion : public b3QuadWord
+{
+public:
+	/**@brief No initialization constructor */
+	b3Quaternion() {}
+
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	// Set Vector
+	B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
+	{
+		mVec128 = vec;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
+	{
+		mVec128 = rhs.mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3Quaternion&
+	operator=(const b3Quaternion& v)
+	{
+		mVec128 = v.mVec128;
+
+		return *this;
+	}
+
+#endif
+
+	//		template <typename b3Scalar>
+	//		explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
+	/**@brief Constructor from scalars */
+	b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+		: b3QuadWord(_x, _y, _z, _w)
+	{
+		//b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
+	}
+	/**@brief Axis angle Constructor
+   * @param axis The axis which the rotation is around
+   * @param angle The magnitude of the rotation around the angle (Radians) */
+	b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
+	{
+		setRotation(_axis, _angle);
+	}
+	/**@brief Constructor from Euler angles
+   * @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
+   * @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
+   * @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
+	b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{
+#ifndef B3_EULER_DEFAULT_ZYX
+		setEuler(yaw, pitch, roll);
+#else
+		setEulerZYX(yaw, pitch, roll);
+#endif
+	}
+	/**@brief Set the rotation using axis angle notation 
+   * @param axis The axis around which to rotate
+   * @param angle The magnitude of the rotation in Radians */
+	void setRotation(const b3Vector3& axis1, const b3Scalar& _angle)
+	{
+		b3Vector3 axis = axis1;
+		axis.safeNormalize();
+		
+		b3Scalar d = axis.length();
+		b3Assert(d != b3Scalar(0.0));
+		if (d < B3_EPSILON)
+		{
+			setValue(0, 0, 0, 1);
+		}
+		else
+		{
+			b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
+			setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
+				b3Cos(_angle * b3Scalar(0.5)));
+		}
+	}
+	/**@brief Set the quaternion using Euler angles
+   * @param yaw Angle around Y
+   * @param pitch Angle around X
+   * @param roll Angle around Z */
+	void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{
+		b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
+		b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
+		b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
+		b3Scalar cosYaw = b3Cos(halfYaw);
+		b3Scalar sinYaw = b3Sin(halfYaw);
+		b3Scalar cosPitch = b3Cos(halfPitch);
+		b3Scalar sinPitch = b3Sin(halfPitch);
+		b3Scalar cosRoll = b3Cos(halfRoll);
+		b3Scalar sinRoll = b3Sin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+				 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+
+	/**@brief Set the quaternion using euler angles 
+   * @param yaw Angle around Z
+   * @param pitch Angle around Y
+   * @param roll Angle around X */
+	void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
+	{
+		b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
+		b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
+		b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
+		b3Scalar cosYaw = b3Cos(halfYaw);
+		b3Scalar sinYaw = b3Sin(halfYaw);
+		b3Scalar cosPitch = b3Cos(halfPitch);
+		b3Scalar sinPitch = b3Sin(halfPitch);
+		b3Scalar cosRoll = b3Cos(halfRoll);
+		b3Scalar sinRoll = b3Sin(halfRoll);
+		setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,   //x
+				 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,   //y
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,   //z
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);  //formerly yzx
+		normalize();
+	}
+
+	/**@brief Get the euler angles from this quaternion
+	   * @param yaw Angle around Z
+	   * @param pitch Angle around Y
+	   * @param roll Angle around X */
+	void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
+	{
+		b3Scalar squ;
+		b3Scalar sqx;
+		b3Scalar sqy;
+		b3Scalar sqz;
+		b3Scalar sarg;
+		sqx = m_floats[0] * m_floats[0];
+		sqy = m_floats[1] * m_floats[1];
+		sqz = m_floats[2] * m_floats[2];
+		squ = m_floats[3] * m_floats[3];
+		rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
+		sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
+		pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI : (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
+		yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
+	}
+
+	/**@brief Add two quaternions
+   * @param q The quaternion to add to this one */
+	B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_add_ps(mVec128, q.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vaddq_f32(mVec128, q.mVec128);
+#else
+		m_floats[0] += q.getX();
+		m_floats[1] += q.getY();
+		m_floats[2] += q.getZ();
+		m_floats[3] += q.m_floats[3];
+#endif
+		return *this;
+	}
+
+	/**@brief Subtract out a quaternion
+   * @param q The quaternion to subtract from this one */
+	b3Quaternion& operator-=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_sub_ps(mVec128, q.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vsubq_f32(mVec128, q.mVec128);
+#else
+		m_floats[0] -= q.getX();
+		m_floats[1] -= q.getY();
+		m_floats[2] -= q.getZ();
+		m_floats[3] -= q.m_floats[3];
+#endif
+		return *this;
+	}
+
+	/**@brief Scale this quaternion
+   * @param s The scalar to scale by */
+	b3Quaternion& operator*=(const b3Scalar& s)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0);     //	(S S S S)
+		mVec128 = _mm_mul_ps(mVec128, vs);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_n_f32(mVec128, s);
+#else
+		m_floats[0] *= s;
+		m_floats[1] *= s;
+		m_floats[2] *= s;
+		m_floats[3] *= s;
+#endif
+		return *this;
+	}
+
+	/**@brief Multiply this quaternion by q on the right
+   * @param q The other quaternion 
+   * Equivilant to this = this * q */
+	b3Quaternion& operator*=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vQ2 = q.get128();
+
+		__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0, 1, 2, 0));
+		__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));
+
+		A1 = A1 * B1;
+
+		__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 1));
+		__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+		A2 = A2 * B2;
+
+		B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2, 0, 1, 2));
+		B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+		B1 = B1 * B2;  //	A3 *= B3
+
+		mVec128 = b3_splat_ps(mVec128, 3);  //	A0
+		mVec128 = mVec128 * vQ2;            //	A0 * B0
+
+		A1 = A1 + A2;                  //	AB12
+		mVec128 = mVec128 - B1;        //	AB03 = AB0 - AB3
+		A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+		mVec128 = mVec128 + A1;        //	AB03 + AB12
+
+#elif defined(B3_USE_NEON)
+
+		float32x4_t vQ1 = mVec128;
+		float32x4_t vQ2 = q.get128();
+		float32x4_t A0, A1, B1, A2, B2, A3, B3;
+		float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+		{
+			float32x2x2_t tmp;
+			tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+			vQ1zx = tmp.val[0];
+
+			tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+			vQ2zx = tmp.val[0];
+		}
+		vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+		vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+		vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+		vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+		A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+		B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+		A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+		B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+		A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+		B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+		A1 = vmulq_f32(A1, B1);
+		A2 = vmulq_f32(A2, B2);
+		A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+		A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+		A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+		A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+		//	change the sign of the last element
+		A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+		A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
+		mVec128 = A0;
+#else
+		setValue(
+			m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
+			m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
+			m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
+			m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
+#endif
+		return *this;
+	}
+	/**@brief Return the dot product between this quaternion and another
+   * @param q The other quaternion */
+	b3Scalar dot(const b3Quaternion& q) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vd;
+
+		vd = _mm_mul_ps(mVec128, q.mVec128);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
+		vd = _mm_add_ps(vd, t);
+		t = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, t);
+
+		return _mm_cvtss_f32(vd);
+#elif defined(B3_USE_NEON)
+		float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
+		x = vpadd_f32(x, x);
+		return vget_lane_f32(x, 0);
+#else
+		return m_floats[0] * q.getX() +
+			   m_floats[1] * q.getY() +
+			   m_floats[2] * q.getZ() +
+			   m_floats[3] * q.m_floats[3];
+#endif
+	}
+
+	/**@brief Return the length squared of the quaternion */
+	b3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+	/**@brief Return the length of the quaternion */
+	b3Scalar length() const
+	{
+		return b3Sqrt(length2());
+	}
+
+	/**@brief Normalize the quaternion 
+   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
+	b3Quaternion& normalize()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vd;
+
+		vd = _mm_mul_ps(mVec128, mVec128);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
+		vd = _mm_add_ps(vd, t);
+		t = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, t);
+
+		vd = _mm_sqrt_ss(vd);
+		vd = _mm_div_ss(b3vOnes, vd);
+		vd = b3_pshufd_ps(vd, 0);  // splat
+		mVec128 = _mm_mul_ps(mVec128, vd);
+
+		return *this;
+#else
+		return *this /= length();
+#endif
+	}
+
+	/**@brief Return a scaled version of this quaternion
+   * @param s The scale factor */
+	B3_FORCE_INLINE b3Quaternion
+	operator*(const b3Scalar& s) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x00);  //	(S S S S)
+
+		return b3Quaternion(_mm_mul_ps(mVec128, vs));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vmulq_n_f32(mVec128, s));
+#else
+		return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
+#endif
+	}
+
+	/**@brief Return an inversely scaled versionof this quaternion
+   * @param s The inverse scale factor */
+	b3Quaternion operator/(const b3Scalar& s) const
+	{
+		b3Assert(s != b3Scalar(0.0));
+		return *this * (b3Scalar(1.0) / s);
+	}
+
+	/**@brief Inversely scale this quaternion
+   * @param s The scale factor */
+	b3Quaternion& operator/=(const b3Scalar& s)
+	{
+		b3Assert(s != b3Scalar(0.0));
+		return *this *= b3Scalar(1.0) / s;
+	}
+
+	/**@brief Return a normalized version of this quaternion */
+	b3Quaternion normalized() const
+	{
+		return *this / length();
+	}
+	/**@brief Return the angle between this quaternion and the other 
+   * @param q The other quaternion */
+	b3Scalar angle(const b3Quaternion& q) const
+	{
+		b3Scalar s = b3Sqrt(length2() * q.length2());
+		b3Assert(s != b3Scalar(0.0));
+		return b3Acos(dot(q) / s);
+	}
+	/**@brief Return the angle of rotation represented by this quaternion */
+	b3Scalar getAngle() const
+	{
+		b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
+		return s;
+	}
+
+	/**@brief Return the axis of the rotation represented by this quaternion */
+	b3Vector3 getAxis() const
+	{
+		b3Scalar s_squared = 1.f - m_floats[3] * m_floats[3];
+
+		if (s_squared < b3Scalar(10.) * B3_EPSILON)  //Check for divide by zero
+			return b3MakeVector3(1.0, 0.0, 0.0);     // Arbitrary
+		b3Scalar s = 1.f / b3Sqrt(s_squared);
+		return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
+	}
+
+	/**@brief Return the inverse of this quaternion */
+	b3Quaternion inverse() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv));
+#else
+		return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the sum of this quaternion and the other 
+   * @param q2 The other quaternion */
+	B3_FORCE_INLINE b3Quaternion
+	operator+(const b3Quaternion& q2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
+#else
+		const b3Quaternion& q1 = *this;
+		return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the difference between this quaternion and the other 
+   * @param q2 The other quaternion */
+	B3_FORCE_INLINE b3Quaternion
+	operator-(const b3Quaternion& q2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
+#else
+		const b3Quaternion& q1 = *this;
+		return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the negative of this quaternion 
+   * This simply negates each element */
+	B3_FORCE_INLINE b3Quaternion operator-() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask));
+#else
+		const b3Quaternion& q2 = *this;
+		return b3Quaternion(-q2.getX(), -q2.getY(), -q2.getZ(), -q2.m_floats[3]);
+#endif
+	}
+	/**@todo document this and it's use */
+	B3_FORCE_INLINE b3Quaternion farthest(const b3Quaternion& qd) const
+	{
+		b3Quaternion diff, sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if (diff.dot(diff) > sum.dot(sum))
+			return qd;
+		return (-qd);
+	}
+
+	/**@todo document this and it's use */
+	B3_FORCE_INLINE b3Quaternion nearest(const b3Quaternion& qd) const
+	{
+		b3Quaternion diff, sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if (diff.dot(diff) < sum.dot(sum))
+			return qd;
+		return (-qd);
+	}
+
+	/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+   * @param q The other quaternion to interpolate with 
+   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
+   * Slerp interpolates assuming constant velocity.  */
+	b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
+	{
+		b3Scalar magnitude = b3Sqrt(length2() * q.length2());
+		b3Assert(magnitude > b3Scalar(0));
+
+		b3Scalar product = dot(q) / magnitude;
+		if (b3Fabs(product) < b3Scalar(1))
+		{
+			// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
+
+			const b3Scalar theta = b3Acos(sign * product);
+			const b3Scalar s1 = b3Sin(sign * t * theta);
+			const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
+			const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
+
+			return b3Quaternion(
+				(m_floats[0] * s0 + q.getX() * s1) * d,
+				(m_floats[1] * s0 + q.getY() * s1) * d,
+				(m_floats[2] * s0 + q.getZ() * s1) * d,
+				(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	static const b3Quaternion& getIdentity()
+	{
+		static const b3Quaternion identityQuat(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.), b3Scalar(1.));
+		return identityQuat;
+	}
+
+	B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
+};
+
+/**@brief Return the product of two quaternions */
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = q1.get128();
+	__m128 vQ2 = q2.get128();
+	__m128 A0, A1, B1, A2, B2;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0));  // X Y  z x     //      vtrn
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));  // W W  W X     // vdup vext
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));  // Y Z  X Y     // vext
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));  // z x  Y Y     // vtrn vdup
+
+	A2 = A2 * B2;
+
+	B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));  // z x Y Z      // vtrn vext
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));  // Y Z x z      // vext vtrn
+
+	B1 = B1 * B2;  //	A3 *= B3
+
+	A0 = b3_splat_ps(vQ1, 3);  //	A0
+	A0 = A0 * vQ2;             //	A0 * B0
+
+	A1 = A1 + A2;  //	AB12
+	A0 = A0 - B1;  //	AB03 = AB0 - AB3
+
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A0 = A0 + A1;                  //	AB03 + AB12
+
+	return b3Quaternion(A0);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = q1.get128();
+	float32x4_t vQ2 = q2.get128();
+	float32x4_t A0, A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+	A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+	A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+	A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
+	return b3Quaternion(A0);
+
+#else
+	return b3Quaternion(
+		q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
+		q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
+		q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
+		q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Quaternion& q, const b3Vector3& w)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = q.get128();
+	__m128 vQ2 = w.get128();
+	__m128 A1, B1, A2, B2, A3, B3;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3, 3, 3, 0));
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0, 1, 2, 0));
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+	A2 = A2 * B2;
+
+	A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
+	B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
+
+	A1 = A1 + A2;                  //	AB12
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = q.get128();
+	float32x4_t vQ2 = w.get128();
+	float32x4_t A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
+
+	vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
+	{
+		float32x2x2_t tmp;
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+	}
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx);  // W W  W X
+	B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx);                     // X Y  z x
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#else
+	return b3Quaternion(
+		q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
+		q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
+		q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
+		-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Vector3& w, const b3Quaternion& q)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = w.get128();
+	__m128 vQ2 = q.get128();
+	__m128 A1, B1, A2, B2, A3, B3;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0));  // X Y  z x
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));  // W W  W X
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+	A2 = A2 * B2;
+
+	A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
+	B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
+
+	A1 = A1 + A2;                  //	AB12
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = w.get128();
+	float32x4_t vQ2 = q.get128();
+	float32x4_t A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
+
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#else
+	return b3Quaternion(
+		+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
+		+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
+		+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
+		-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
+#endif
+}
+
+/**@brief Calculate the dot product between two quaternions */
+B3_FORCE_INLINE b3Scalar
+b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+	return q1.dot(q2);
+}
+
+/**@brief Return the length of a quaternion */
+B3_FORCE_INLINE b3Scalar
+b3Length(const b3Quaternion& q)
+{
+	return q.length();
+}
+
+/**@brief Return the angle between two quaternions*/
+B3_FORCE_INLINE b3Scalar
+b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+	return q1.angle(q2);
+}
+
+/**@brief Return the inverse of a quaternion*/
+B3_FORCE_INLINE b3Quaternion
+b3Inverse(const b3Quaternion& q)
+{
+	return q.inverse();
+}
+
+/**@brief Return the result of spherical linear interpolation betwen two quaternions 
+ * @param q1 The first quaternion
+ * @param q2 The second quaternion 
+ * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
+ * Slerp assumes constant velocity between positions. */
+B3_FORCE_INLINE b3Quaternion
+b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
+{
+	return q1.slerp(q2, t);
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
+{
+	return rot0 * rot1;
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3QuatNormalized(const b3Quaternion& orn)
+{
+	return orn.normalized();
+}
+
+B3_FORCE_INLINE b3Vector3
+b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
+{
+	b3Quaternion q = rotation * v;
+	q *= rotation.inverse();
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
+#else
+	return b3MakeVector3(q.getX(), q.getY(), q.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1)  // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	b3Vector3 c = v0.cross(v1);
+	b3Scalar d = v0.dot(v1);
+
+	if (d < -1.0 + B3_EPSILON)
+	{
+		b3Vector3 n, unused;
+		b3PlaneSpace1(v0, n, unused);
+		return b3Quaternion(n.getX(), n.getY(), n.getZ(), 0.0f);  // just pick any vector that is orthogonal to v0
+	}
+
+	b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
+	b3Scalar rs = 1.0f / s;
+
+	return b3Quaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3ShortestArcQuatNormalize2(b3Vector3& v0, b3Vector3& v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return b3ShortestArcQuat(v0, v1);
+}
+
+#endif  //B3_SIMD__QUATERNION_H_

Engine/lib/bullet/src/Bullet3Common/b3Random.h

@@ -0,0 +1,46 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_GEN_RANDOM_H
+#define B3_GEN_RANDOM_H
+
+#include "b3Scalar.h"
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define B3_RAND_MAX UINT_MAX
+
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define B3_RAND_MAX RAND_MAX
+
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
+
+#endif
+
+inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
+{
+	return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
+}
+
+#endif  //B3_GEN_RANDOM_H

Engine/lib/bullet/src/Bullet3Common/b3ResizablePool.h

@@ -0,0 +1,171 @@
+
+#ifndef B3_RESIZABLE_POOL_H
+#define B3_RESIZABLE_POOL_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+enum
+{
+	B3_POOL_HANDLE_TERMINAL_FREE = -1,
+	B3_POOL_HANDLE_TERMINAL_USED = -2
+};
+
+template <typename U>
+struct b3PoolBodyHandle : public U
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_nextFreeHandle;
+	void setNextFree(int next)
+	{
+		m_nextFreeHandle = next;
+	}
+	int getNextFree() const
+	{
+		return m_nextFreeHandle;
+	}
+};
+
+template <typename T>
+class b3ResizablePool
+{
+protected:
+	b3AlignedObjectArray<T> m_bodyHandles;
+	int m_numUsedHandles;   // number of active handles
+	int m_firstFreeHandle;  // free handles list
+
+	T* getHandleInternal(int handle)
+	{
+		return &m_bodyHandles[handle];
+	}
+	const T* getHandleInternal(int handle) const
+	{
+		return &m_bodyHandles[handle];
+	}
+
+public:
+	b3ResizablePool()
+	{
+		initHandles();
+	}
+
+	virtual ~b3ResizablePool()
+	{
+		exitHandles();
+	}
+	///handle management
+
+	int getNumHandles() const
+	{
+		return m_bodyHandles.size();
+	}
+
+	void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
+	{
+		for (int i = 0; i < m_bodyHandles.size(); i++)
+		{
+			if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+			{
+				usedHandles.push_back(i);
+			}
+		}
+	}
+
+	T* getHandle(int handle)
+	{
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
+		{
+			return 0;
+		}
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			return &m_bodyHandles[handle];
+		}
+		return 0;
+	}
+	const T* getHandle(int handle) const
+	{
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
+		{
+			return 0;
+		}
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			return &m_bodyHandles[handle];
+		}
+		return 0;
+	}
+
+	void increaseHandleCapacity(int extraCapacity)
+	{
+		int curCapacity = m_bodyHandles.size();
+		//b3Assert(curCapacity == m_numUsedHandles);
+		int newCapacity = curCapacity + extraCapacity;
+		m_bodyHandles.resize(newCapacity);
+
+		{
+			for (int i = curCapacity; i < newCapacity; i++)
+				m_bodyHandles[i].setNextFree(i + 1);
+
+			m_bodyHandles[newCapacity - 1].setNextFree(-1);
+		}
+		m_firstFreeHandle = curCapacity;
+	}
+	void initHandles()
+	{
+		m_numUsedHandles = 0;
+		m_firstFreeHandle = -1;
+
+		increaseHandleCapacity(1);
+	}
+
+	void exitHandles()
+	{
+		m_bodyHandles.resize(0);
+		m_firstFreeHandle = -1;
+		m_numUsedHandles = 0;
+	}
+
+	int allocHandle()
+	{
+		b3Assert(m_firstFreeHandle >= 0);
+
+		int handle = m_firstFreeHandle;
+		m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
+		m_numUsedHandles++;
+
+		if (m_firstFreeHandle < 0)
+		{
+			//int curCapacity = m_bodyHandles.size();
+			int additionalCapacity = m_bodyHandles.size();
+			increaseHandleCapacity(additionalCapacity);
+
+			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
+		}
+		getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
+		getHandleInternal(handle)->clear();
+		return handle;
+	}
+
+	void freeHandle(int handle)
+	{
+		b3Assert(handle >= 0);
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			getHandleInternal(handle)->clear();
+			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
+			m_firstFreeHandle = handle;
+			m_numUsedHandles--;
+		}
+	}
+};
+///end handle management
+
+#endif  //B3_RESIZABLE_POOL_H

Engine/lib/bullet/src/Bullet3Common/b3Scalar.h

@@ -0,0 +1,685 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_SCALAR_H
+#define B3_SCALAR_H
+
+#ifdef B3_MANAGED_CODE
+//Aligned data types not supported in managed code
+#pragma unmanaged
+#endif
+
+#include <math.h>
+#include <stdlib.h>  //size_t for MSVC 6.0
+#include <float.h>
+
+//Original repository is at http://github.com/erwincoumans/bullet3
+#define B3_BULLET_VERSION 300
+
+inline int b3GetVersion()
+{
+	return B3_BULLET_VERSION;
+}
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define B3_DEBUG
+#endif
+
+#include "b3Logging.h"  //for b3Error
+
+#ifdef _WIN32
+
+#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
+
+#define B3_FORCE_INLINE inline
+#define B3_ATTRIBUTE_ALIGNED16(a) a
+#define B3_ATTRIBUTE_ALIGNED64(a) a
+#define B3_ATTRIBUTE_ALIGNED128(a) a
+#else
+//#define B3_HAS_ALIGNED_ALLOCATOR
+#pragma warning(disable : 4324)  // disable padding warning
+//			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
+#pragma warning(disable : 4996)  //Turn off warnings about deprecated C routines
+//			#pragma warning(disable:4786) // Disable the "debug name too long" warning
+
+#define B3_FORCE_INLINE __forceinline
+#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
+#define B3_ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
+#ifdef _XBOX
+#define B3_USE_VMX128
+
+#include <ppcintrinsics.h>
+#define B3_HAVE_NATIVE_FSEL
+#define b3Fsel(a, b, c) __fsel((a), (b), (c))
+#else
+
+#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
+#if (defined(_M_IX86) || defined(_M_X64))
+#define B3_USE_SSE
+#ifdef B3_USE_SSE
+//B3_USE_SSE_IN_API is disabled under Windows by default, because
+//it makes it harder to integrate Bullet into your application under Windows
+//(structured embedding Bullet structs/classes need to be 16-byte aligned)
+//with relatively little performance gain
+//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
+//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
+//#define B3_USE_SSE_IN_API
+#endif  //B3_USE_SSE
+#include <emmintrin.h>
+#endif
+#endif
+
+#endif  //_XBOX
+
+#endif  //__MINGW32__
+
+#ifdef B3_DEBUG
+#ifdef _MSC_VER
+#include <stdio.h>
+#define b3Assert(x)               \
+	{                             \
+		if (!(x))                 \
+		{                         \
+			b3Error(              \
+				"Assert "__FILE__ \
+				":%u (" #x ")\n", \
+				__LINE__);        \
+			__debugbreak();       \
+		}                         \
+	}
+#else  //_MSC_VER
+#include <assert.h>
+#define b3Assert assert
+#endif  //_MSC_VER
+#else
+#define b3Assert(x)
+#endif
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
+
+#else
+
+#if defined(__CELLOS_LV2__)
+#define B3_FORCE_INLINE inline __attribute__((always_inline))
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+#ifdef B3_DEBUG
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#define b3Assert(x)               \
+	{                             \
+		if (!(x))                 \
+		{                         \
+			b3Error(              \
+				"Assert "__FILE__ \
+				":%u (" #x ")\n", \
+				__LINE__);        \
+			spu_hcmpeq(0, 0);     \
+		}                         \
+	}
+#else
+#define b3Assert assert
+#endif
+
+#else
+#define b3Assert(x)
+#endif
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
+
+#else
+
+#ifdef USE_LIBSPE2
+
+#define B3_FORCE_INLINE __inline
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+#ifdef B3_DEBUG
+#define b3Assert assert
+#else
+#define b3Assert(x)
+#endif
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+
+#define b3Likely(_c) __builtin_expect((_c), 1)
+#define b3Unlikely(_c) __builtin_expect((_c), 0)
+
+#else
+//non-windows systems
+
+#if (defined(__APPLE__) && (!defined(B3_USE_DOUBLE_PRECISION)))
+#if defined(__i386__) || defined(__x86_64__)
+#define B3_USE_SSE
+//B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
+//if apps run into issues, we will disable the next line
+#define B3_USE_SSE_IN_API
+#ifdef B3_USE_SSE
+// include appropriate SSE level
+#if defined(__SSE4_1__)
+#include <smmintrin.h>
+#elif defined(__SSSE3__)
+#include <tmmintrin.h>
+#elif defined(__SSE3__)
+#include <pmmintrin.h>
+#else
+#include <emmintrin.h>
+#endif
+#endif  //B3_USE_SSE
+#elif defined(__armv7__)
+#ifdef __clang__
+#define B3_USE_NEON 1
+
+#if defined B3_USE_NEON && defined(__clang__)
+#include <arm_neon.h>
+#endif  //B3_USE_NEON
+#endif  //__clang__
+#endif  //__arm__
+
+#define B3_FORCE_INLINE inline __attribute__((always_inline))
+///@todo: check out alignment methods for other platforms/compilers
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+
+#if defined(DEBUG) || defined(_DEBUG)
+#if defined(__i386__) || defined(__x86_64__)
+#include <stdio.h>
+#define b3Assert(x)                                                             \
+	{                                                                           \
+		if (!(x))                                                               \
+		{                                                                       \
+			b3Error("Assert %s in line %d, file %s\n", #x, __LINE__, __FILE__); \
+			asm volatile("int3");                                               \
+		}                                                                       \
+	}
+#else  //defined (__i386__) || defined (__x86_64__)
+#define b3Assert assert
+#endif  //defined (__i386__) || defined (__x86_64__)
+#else   //defined(DEBUG) || defined (_DEBUG)
+#define b3Assert(x)
+#endif  //defined(DEBUG) || defined (_DEBUG)
+
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
+
+#else
+
+#define B3_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+///#define B3_ATTRIBUTE_ALIGNED16(a) a
+///#define B3_ATTRIBUTE_ALIGNED64(a) a
+///#define B3_ATTRIBUTE_ALIGNED128(a) a
+#ifndef assert
+#include <assert.h>
+#endif
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define b3Assert assert
+#else
+#define b3Assert(x)
+#endif
+
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
+#endif  //__APPLE__
+
+#endif  // LIBSPE2
+
+#endif  //__CELLOS_LV2__
+#endif
+
+///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
+#if defined(B3_USE_DOUBLE_PRECISION)
+typedef double b3Scalar;
+//this number could be bigger in double precision
+#define B3_LARGE_FLOAT 1e30
+#else
+typedef float b3Scalar;
+//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
+#define B3_LARGE_FLOAT 1e18f
+#endif
+
+#ifdef B3_USE_SSE
+typedef __m128 b3SimdFloat4;
+#endif  //B3_USE_SSE
+
+#if defined B3_USE_SSE_IN_API && defined(B3_USE_SSE)
+#ifdef _WIN32
+
+#ifndef B3_NAN
+static int b3NanMask = 0x7F800001;
+#define B3_NAN (*(float *)&b3NanMask)
+#endif
+
+#ifndef B3_INFINITY_MASK
+static int b3InfinityMask = 0x7F800000;
+#define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
+#endif
+
+inline __m128 operator+(const __m128 A, const __m128 B)
+{
+	return _mm_add_ps(A, B);
+}
+
+inline __m128 operator-(const __m128 A, const __m128 B)
+{
+	return _mm_sub_ps(A, B);
+}
+
+inline __m128 operator*(const __m128 A, const __m128 B)
+{
+	return _mm_mul_ps(A, B);
+}
+
+#define b3CastfTo128i(a) (_mm_castps_si128(a))
+#define b3CastfTo128d(a) (_mm_castps_pd(a))
+#define b3CastiTo128f(a) (_mm_castsi128_ps(a))
+#define b3CastdTo128f(a) (_mm_castpd_ps(a))
+#define b3CastdTo128i(a) (_mm_castpd_si128(a))
+#define b3Assign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
+
+#else  //_WIN32
+
+#define b3CastfTo128i(a) ((__m128i)(a))
+#define b3CastfTo128d(a) ((__m128d)(a))
+#define b3CastiTo128f(a) ((__m128)(a))
+#define b3CastdTo128f(a) ((__m128)(a))
+#define b3CastdTo128i(a) ((__m128i)(a))
+#define b3Assign128(r0, r1, r2, r3) \
+	(__m128) { r0, r1, r2, r3 }
+#endif  //_WIN32
+#endif  //B3_USE_SSE_IN_API
+
+#ifdef B3_USE_NEON
+#include <arm_neon.h>
+
+typedef float32x4_t b3SimdFloat4;
+#define B3_INFINITY INFINITY
+#define B3_NAN NAN
+#define b3Assign128(r0, r1, r2, r3) \
+	(float32x4_t) { r0, r1, r2, r3 }
+#endif
+
+#define B3_DECLARE_ALIGNED_ALLOCATOR()                                                                   \
+	B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); }   \
+	B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); }                              \
+	B3_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
+	B3_FORCE_INLINE void operator delete(void *, void *) {}                                              \
+	B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
+	B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); }                            \
+	B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
+	B3_FORCE_INLINE void operator delete[](void *, void *) {}
+
+#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
+
+B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
+{
+	return sqrt(x);
+}
+B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
+B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
+B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
+B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
+B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
+{
+	if (x < b3Scalar(-1)) x = b3Scalar(-1);
+	if (x > b3Scalar(1)) x = b3Scalar(1);
+	return acos(x);
+}
+B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
+{
+	if (x < b3Scalar(-1)) x = b3Scalar(-1);
+	if (x > b3Scalar(1)) x = b3Scalar(1);
+	return asin(x);
+}
+B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
+B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
+B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
+B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
+B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
+B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
+
+#else
+
+B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
+{
+#ifdef USE_APPROXIMATION
+	double x, z, tempf;
+	unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
+
+	tempf = y;
+	*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
+	x = tempf;
+	z = y * b3Scalar(0.5);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	return x * y;
+#else
+	return sqrtf(y);
+#endif
+}
+B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
+B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
+B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
+B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
+B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
+{
+	if (x < b3Scalar(-1))
+		x = b3Scalar(-1);
+	if (x > b3Scalar(1))
+		x = b3Scalar(1);
+	return acosf(x);
+}
+B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
+{
+	if (x < b3Scalar(-1))
+		x = b3Scalar(-1);
+	if (x > b3Scalar(1))
+		x = b3Scalar(1);
+	return asinf(x);
+}
+B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
+B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
+B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
+B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
+B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
+B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmodf(x, y); }
+
+#endif
+
+#define B3_2_PI b3Scalar(6.283185307179586232)
+#define B3_PI (B3_2_PI * b3Scalar(0.5))
+#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
+#define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0))
+#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
+#define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490)
+
+#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define B3_EPSILON DBL_EPSILON
+#define B3_INFINITY DBL_MAX
+#else
+#define B3_EPSILON FLT_EPSILON
+#define B3_INFINITY FLT_MAX
+#endif
+
+B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
+{
+	b3Scalar coeff_1 = B3_PI / 4.0f;
+	b3Scalar coeff_2 = 3.0f * coeff_1;
+	b3Scalar abs_y = b3Fabs(y);
+	b3Scalar angle;
+	if (x >= 0.0f)
+	{
+		b3Scalar r = (x - abs_y) / (x + abs_y);
+		angle = coeff_1 - coeff_1 * r;
+	}
+	else
+	{
+		b3Scalar r = (x + abs_y) / (abs_y - x);
+		angle = coeff_2 - coeff_1 * r;
+	}
+	return (y < 0.0f) ? -angle : angle;
+}
+
+B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
+
+B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
+{
+	return (((a) <= eps) && !((a) < -eps));
+}
+B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
+{
+	return (!((a) <= eps));
+}
+
+B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
+{
+	return x < b3Scalar(0.0) ? 1 : 0;
+}
+
+B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
+B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
+
+#define B3_DECLARE_HANDLE(name) \
+	typedef struct name##__     \
+	{                           \
+		int unused;             \
+	} * name
+
+#ifndef b3Fsel
+B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
+{
+	return a >= 0 ? b : c;
+}
+#endif
+#define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
+
+B3_FORCE_INLINE bool b3MachineIsLittleEndian()
+{
+	long int i = 1;
+	const char *p = (const char *)&i;
+	if (p[0] == 1)  // Lowest address contains the least significant byte
+		return true;
+	else
+		return false;
+}
+
+///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
+{
+	// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+	// Rely on positive value or'ed with its negative having sign bit on
+	// and zero value or'ed with its negative (which is still zero) having sign bit off
+	// Use arithmetic shift right, shifting the sign bit through all 32 bits
+	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+	unsigned testEqz = ~testNz;
+	return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+B3_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+	unsigned testEqz = ~testNz;
+	return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef B3_HAVE_NATIVE_FSEL
+	return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+	return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
+#endif
+}
+
+template <typename T>
+B3_FORCE_INLINE void b3Swap(T &a, T &b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+//PCK: endian swapping functions
+B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
+{
+	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
+}
+
+B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
+{
+	return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+}
+
+B3_FORCE_INLINE unsigned b3SwapEndian(int val)
+{
+	return b3SwapEndian((unsigned)val);
+}
+
+B3_FORCE_INLINE unsigned short b3SwapEndian(short val)
+{
+	return b3SwapEndian((unsigned short)val);
+}
+
+///b3SwapFloat uses using char pointers to swap the endianness
+////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
+///so instead of returning a float/double, we return integer/long long integer
+B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
+{
+	unsigned int a = 0;
+	unsigned char *dst = (unsigned char *)&a;
+	unsigned char *src = (unsigned char *)&d;
+
+	dst[0] = src[3];
+	dst[1] = src[2];
+	dst[2] = src[1];
+	dst[3] = src[0];
+	return a;
+}
+
+// unswap using char pointers
+B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
+{
+	float d = 0.0f;
+	unsigned char *src = (unsigned char *)&a;
+	unsigned char *dst = (unsigned char *)&d;
+
+	dst[0] = src[3];
+	dst[1] = src[2];
+	dst[2] = src[1];
+	dst[3] = src[0];
+
+	return d;
+}
+
+// swap using char pointers
+B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char *dst)
+{
+	unsigned char *src = (unsigned char *)&d;
+
+	dst[0] = src[7];
+	dst[1] = src[6];
+	dst[2] = src[5];
+	dst[3] = src[4];
+	dst[4] = src[3];
+	dst[5] = src[2];
+	dst[6] = src[1];
+	dst[7] = src[0];
+}
+
+// unswap using char pointers
+B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
+{
+	double d = 0.0;
+	unsigned char *dst = (unsigned char *)&d;
+
+	dst[0] = src[7];
+	dst[1] = src[6];
+	dst[2] = src[5];
+	dst[3] = src[4];
+	dst[4] = src[3];
+	dst[5] = src[2];
+	dst[6] = src[1];
+	dst[7] = src[0];
+
+	return d;
+}
+
+// returns normalized value in range [-B3_PI, B3_PI]
+B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
+{
+	angleInRadians = b3Fmod(angleInRadians, B3_2_PI);
+	if (angleInRadians < -B3_PI)
+	{
+		return angleInRadians + B3_2_PI;
+	}
+	else if (angleInRadians > B3_PI)
+	{
+		return angleInRadians - B3_2_PI;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+///rudimentary class to provide type info
+struct b3TypedObject
+{
+	b3TypedObject(int objectType)
+		: m_objectType(objectType)
+	{
+	}
+	int m_objectType;
+	inline int getObjectType() const
+	{
+		return m_objectType;
+	}
+};
+
+///align a pointer to the provided alignment, upwards
+template <typename T>
+T *b3AlignPointer(T *unalignedPtr, size_t alignment)
+{
+	struct b3ConvertPointerSizeT
+	{
+		union {
+			T *ptr;
+			size_t integer;
+		};
+	};
+	b3ConvertPointerSizeT converter;
+
+	const size_t bit_mask = ~(alignment - 1);
+	converter.ptr = unalignedPtr;
+	converter.integer += alignment - 1;
+	converter.integer &= bit_mask;
+	return converter.ptr;
+}
+
+#endif  //B3_SCALAR_H

Engine/lib/bullet/src/Bullet3Common/b3StackAlloc.h

@@ -0,0 +1,118 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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.
+*/
+
+/*
+StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+#ifndef B3_STACK_ALLOC
+#define B3_STACK_ALLOC
+
+#include "b3Scalar.h"  //for b3Assert
+#include "b3AlignedAllocator.h"
+
+///The b3Block class is an internal structure for the b3StackAlloc memory allocator.
+struct b3Block
+{
+	b3Block* previous;
+	unsigned char* address;
+};
+
+///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
+class b3StackAlloc
+{
+public:
+	b3StackAlloc(unsigned int size)
+	{
+		ctor();
+		create(size);
+	}
+	~b3StackAlloc() { destroy(); }
+
+	inline void create(unsigned int size)
+	{
+		destroy();
+		data = (unsigned char*)b3AlignedAlloc(size, 16);
+		totalsize = size;
+	}
+	inline void destroy()
+	{
+		b3Assert(usedsize == 0);
+		//Raise(L"StackAlloc is still in use");
+
+		if (usedsize == 0)
+		{
+			if (!ischild && data)
+				b3AlignedFree(data);
+
+			data = 0;
+			usedsize = 0;
+		}
+	}
+
+	int getAvailableMemory() const
+	{
+		return static_cast<int>(totalsize - usedsize);
+	}
+
+	unsigned char* allocate(unsigned int size)
+	{
+		const unsigned int nus(usedsize + size);
+		if (nus < totalsize)
+		{
+			usedsize = nus;
+			return (data + (usedsize - size));
+		}
+		b3Assert(0);
+		//&& (L"Not enough memory"));
+
+		return (0);
+	}
+	B3_FORCE_INLINE b3Block* beginBlock()
+	{
+		b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
+		pb->previous = current;
+		pb->address = data + usedsize;
+		current = pb;
+		return (pb);
+	}
+	B3_FORCE_INLINE void endBlock(b3Block* block)
+	{
+		b3Assert(block == current);
+		//Raise(L"Unmatched blocks");
+		if (block == current)
+		{
+			current = block->previous;
+			usedsize = (unsigned int)((block->address - data) - sizeof(b3Block));
+		}
+	}
+
+private:
+	void ctor()
+	{
+		data = 0;
+		totalsize = 0;
+		usedsize = 0;
+		current = 0;
+		ischild = false;
+	}
+	unsigned char* data;
+	unsigned int totalsize;
+	unsigned int usedsize;
+	b3Block* current;
+	bool ischild;
+};
+
+#endif  //B3_STACK_ALLOC

Engine/lib/bullet/src/Bullet3Common/b3Transform.h

@@ -0,0 +1,286 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_TRANSFORM_H
+#define B3_TRANSFORM_H
+
+#include "b3Matrix3x3.h"
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3TransformData b3TransformDoubleData
+#else
+#define b3TransformData b3TransformFloatData
+#endif
+
+/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
+ *It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Transform
+{
+	///Storage for the rotation
+	b3Matrix3x3 m_basis;
+	///Storage for the translation
+	b3Vector3 m_origin;
+
+public:
+	/**@brief No initialization constructor */
+	b3Transform() {}
+	/**@brief Constructor from b3Quaternion (optional b3Vector3 )
+   * @param q Rotation from quaternion 
+   * @param c Translation from Vector (default 0,0,0) */
+	explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q,
+										 const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
+		: m_basis(q),
+		  m_origin(c)
+	{
+	}
+
+	/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
+   * @param b Rotation from Matrix 
+   * @param c Translation from Vector default (0,0,0)*/
+	explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
+										 const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
+		: m_basis(b),
+		  m_origin(c)
+	{
+	}
+	/**@brief Copy constructor */
+	B3_FORCE_INLINE b3Transform(const b3Transform& other)
+		: m_basis(other.m_basis),
+		  m_origin(other.m_origin)
+	{
+	}
+	/**@brief Assignment Operator */
+	B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other)
+	{
+		m_basis = other.m_basis;
+		m_origin = other.m_origin;
+		return *this;
+	}
+
+	/**@brief Set the current transform as the value of the product of two transforms
+   * @param t1 Transform 1
+   * @param t2 Transform 2
+   * This = Transform1 * Transform2 */
+	B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2)
+	{
+		m_basis = t1.m_basis * t2.m_basis;
+		m_origin = t1(t2.m_origin);
+	}
+
+	/*		void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
+			b3Vector3 v = t2.m_origin - t1.m_origin;
+			m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis);
+			m_origin = v * t1.m_basis;
+		}
+		*/
+
+	/**@brief Return the transform of the vector */
+	B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const
+	{
+		return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
+	}
+
+	/**@brief Return the transform of the vector */
+	B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const
+	{
+		return (*this)(x);
+	}
+
+	/**@brief Return the transform of the b3Quaternion */
+	B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const
+	{
+		return getRotation() * q;
+	}
+
+	/**@brief Return the basis matrix for the rotation */
+	B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
+	/**@brief Return the basis matrix for the rotation */
+	B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
+
+	/**@brief Return the origin vector translation */
+	B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
+	/**@brief Return the origin vector translation */
+	B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
+
+	/**@brief Return a quaternion representing the rotation */
+	b3Quaternion getRotation() const
+	{
+		b3Quaternion q;
+		m_basis.getRotation(q);
+		return q;
+	}
+
+	/**@brief Set from an array 
+   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+	void setFromOpenGLMatrix(const b3Scalar* m)
+	{
+		m_basis.setFromOpenGLSubMatrix(m);
+		m_origin.setValue(m[12], m[13], m[14]);
+	}
+
+	/**@brief Fill an array representation
+   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+	void getOpenGLMatrix(b3Scalar * m) const
+	{
+		m_basis.getOpenGLSubMatrix(m);
+		m[12] = m_origin.getX();
+		m[13] = m_origin.getY();
+		m[14] = m_origin.getZ();
+		m[15] = b3Scalar(1.0);
+	}
+
+	/**@brief Set the translational element
+   * @param origin The vector to set the translation to */
+	B3_FORCE_INLINE void setOrigin(const b3Vector3& origin)
+	{
+		m_origin = origin;
+	}
+
+	B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
+
+	/**@brief Set the rotational element by b3Matrix3x3 */
+	B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
+	{
+		m_basis = basis;
+	}
+
+	/**@brief Set the rotational element by b3Quaternion */
+	B3_FORCE_INLINE void setRotation(const b3Quaternion& q)
+	{
+		m_basis.setRotation(q);
+	}
+
+	/**@brief Set this transformation to the identity */
+	void setIdentity()
+	{
+		m_basis.setIdentity();
+		m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0));
+	}
+
+	/**@brief Multiply this Transform by another(this = this * another) 
+   * @param t The other transform */
+	b3Transform& operator*=(const b3Transform& t)
+	{
+		m_origin += m_basis * t.m_origin;
+		m_basis *= t.m_basis;
+		return *this;
+	}
+
+	/**@brief Return the inverse of this transform */
+	b3Transform inverse() const
+	{
+		b3Matrix3x3 inv = m_basis.transpose();
+		return b3Transform(inv, inv * -m_origin);
+	}
+
+	/**@brief Return the inverse of this transform times the other transform
+   * @param t The other transform 
+   * return this.inverse() * the other */
+	b3Transform inverseTimes(const b3Transform& t) const;
+
+	/**@brief Return the product of this transform and the other */
+	b3Transform operator*(const b3Transform& t) const;
+
+	/**@brief Return an identity transform */
+	static const b3Transform& getIdentity()
+	{
+		static const b3Transform identityTransform(b3Matrix3x3::getIdentity());
+		return identityTransform;
+	}
+
+	void serialize(struct b3TransformData & dataOut) const;
+
+	void serializeFloat(struct b3TransformFloatData & dataOut) const;
+
+	void deSerialize(const struct b3TransformData& dataIn);
+
+	void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
+
+	void deSerializeFloat(const struct b3TransformFloatData& dataIn);
+};
+
+B3_FORCE_INLINE b3Vector3
+b3Transform::invXform(const b3Vector3& inVec) const
+{
+	b3Vector3 v = inVec - m_origin;
+	return (m_basis.transpose() * v);
+}
+
+B3_FORCE_INLINE b3Transform
+b3Transform::inverseTimes(const b3Transform& t) const
+{
+	b3Vector3 v = t.getOrigin() - m_origin;
+	return b3Transform(m_basis.transposeTimes(t.m_basis),
+					   v * m_basis);
+}
+
+B3_FORCE_INLINE b3Transform
+	b3Transform::operator*(const b3Transform& t) const
+{
+	return b3Transform(m_basis * t.m_basis,
+					   (*this)(t.m_origin));
+}
+
+/**@brief Test if two transforms have all elements equal */
+B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
+{
+	return (t1.getBasis() == t2.getBasis() &&
+			t1.getOrigin() == t2.getOrigin());
+}
+
+///for serialization
+struct b3TransformFloatData
+{
+	b3Matrix3x3FloatData m_basis;
+	b3Vector3FloatData m_origin;
+};
+
+struct b3TransformDoubleData
+{
+	b3Matrix3x3DoubleData m_basis;
+	b3Vector3DoubleData m_origin;
+};
+
+B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
+{
+	m_basis.serialize(dataOut.m_basis);
+	m_origin.serialize(dataOut.m_origin);
+}
+
+B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
+{
+	m_basis.serializeFloat(dataOut.m_basis);
+	m_origin.serializeFloat(dataOut.m_origin);
+}
+
+B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
+{
+	m_basis.deSerialize(dataIn.m_basis);
+	m_origin.deSerialize(dataIn.m_origin);
+}
+
+B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
+{
+	m_basis.deSerializeFloat(dataIn.m_basis);
+	m_origin.deSerializeFloat(dataIn.m_origin);
+}
+
+B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
+{
+	m_basis.deSerializeDouble(dataIn.m_basis);
+	m_origin.deSerializeDouble(dataIn.m_origin);
+}
+
+#endif  //B3_TRANSFORM_H

Engine/lib/bullet/src/Bullet3Common/b3TransformUtil.h

@@ -0,0 +1,210 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_TRANSFORM_UTIL_H
+#define B3_TRANSFORM_UTIL_H
+
+#include "b3Transform.h"
+#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
+
+B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
+{
+	return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
+						 supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
+						 supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
+}
+
+/// Utils related to temporal transforms
+class b3TransformUtil
+{
+public:
+	static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
+	{
+		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
+		//	#define QUATERNION_DERIVATIVE
+#ifdef QUATERNION_DERIVATIVE
+		b3Quaternion predictedOrn = curTrans.getRotation();
+		predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
+		predictedOrn.normalize();
+#else
+		//Exponential map
+		//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
+
+		b3Vector3 axis;
+		b3Scalar fAngle = angvel.length();
+		//limit the angular motion
+		if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
+		{
+			fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
+		}
+
+		if (fAngle < b3Scalar(0.001))
+		{
+			// use Taylor's expansions of sync function
+			axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
+		}
+		else
+		{
+			// sync(fAngle) = sin(c*fAngle)/t
+			axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
+		}
+		b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
+		b3Quaternion orn0 = curTrans.getRotation();
+
+		b3Quaternion predictedOrn = dorn * orn0;
+		predictedOrn.normalize();
+#endif
+		predictedTransform.setRotation(predictedOrn);
+	}
+
+	static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
+	{
+		linVel = (pos1 - pos0) / timeStep;
+		b3Vector3 axis;
+		b3Scalar angle;
+		if (orn0 != orn1)
+		{
+			calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
+			angVel = axis * angle / timeStep;
+		}
+		else
+		{
+			angVel.setValue(0, 0, 0);
+		}
+	}
+
+	static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
+	{
+		b3Quaternion orn1 = orn0.nearest(orn1a);
+		b3Quaternion dorn = orn1 * orn0.inverse();
+		angle = dorn.getAngle();
+		axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
+		axis[3] = b3Scalar(0.);
+		//check for axis length
+		b3Scalar len = axis.length2();
+		if (len < B3_EPSILON * B3_EPSILON)
+			axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
+		else
+			axis /= b3Sqrt(len);
+	}
+
+	static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
+	{
+		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
+		b3Vector3 axis;
+		b3Scalar angle;
+		calculateDiffAxisAngle(transform0, transform1, axis, angle);
+		angVel = axis * angle / timeStep;
+	}
+
+	static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
+	{
+		b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
+		b3Quaternion dorn;
+		dmat.getRotation(dorn);
+
+		///floating point inaccuracy can lead to w component > 1..., which breaks
+		dorn.normalize();
+
+		angle = dorn.getAngle();
+		axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
+		axis[3] = b3Scalar(0.);
+		//check for axis length
+		b3Scalar len = axis.length2();
+		if (len < B3_EPSILON * B3_EPSILON)
+			axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
+		else
+			axis /= b3Sqrt(len);
+	}
+};
+
+///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
+///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
+class b3ConvexSeparatingDistanceUtil
+{
+	b3Quaternion m_ornA;
+	b3Quaternion m_ornB;
+	b3Vector3 m_posA;
+	b3Vector3 m_posB;
+
+	b3Vector3 m_separatingNormal;
+
+	b3Scalar m_boundingRadiusA;
+	b3Scalar m_boundingRadiusB;
+	b3Scalar m_separatingDistance;
+
+public:
+	b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
+		: m_boundingRadiusA(boundingRadiusA),
+		  m_boundingRadiusB(boundingRadiusB),
+		  m_separatingDistance(0.f)
+	{
+	}
+
+	b3Scalar getConservativeSeparatingDistance()
+	{
+		return m_separatingDistance;
+	}
+
+	void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
+	{
+		const b3Vector3& toPosA = transA.getOrigin();
+		const b3Vector3& toPosB = transB.getOrigin();
+		b3Quaternion toOrnA = transA.getRotation();
+		b3Quaternion toOrnB = transB.getRotation();
+
+		if (m_separatingDistance > 0.f)
+		{
+			b3Vector3 linVelA, angVelA, linVelB, angVelB;
+			b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
+			b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
+			b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
+			b3Vector3 relLinVel = (linVelB - linVelA);
+			b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
+			if (relLinVelocLength < 0.f)
+			{
+				relLinVelocLength = 0.f;
+			}
+
+			b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
+			m_separatingDistance -= projectedMotion;
+		}
+
+		m_posA = toPosA;
+		m_posB = toPosB;
+		m_ornA = toOrnA;
+		m_ornB = toOrnB;
+	}
+
+	void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
+	{
+		m_separatingDistance = separatingDistance;
+
+		if (m_separatingDistance > 0.f)
+		{
+			m_separatingNormal = separatingVector;
+
+			const b3Vector3& toPosA = transA.getOrigin();
+			const b3Vector3& toPosB = transB.getOrigin();
+			b3Quaternion toOrnA = transA.getRotation();
+			b3Quaternion toOrnB = transB.getRotation();
+			m_posA = toPosA;
+			m_posB = toPosB;
+			m_ornA = toOrnA;
+			m_ornB = toOrnB;
+		}
+	}
+};
+
+#endif  //B3_TRANSFORM_UTIL_H

Engine/lib/bullet/src/Bullet3Common/b3Vector3.cpp

@@ -0,0 +1,1637 @@
+/*
+ Copyright (c) 2011-213 Apple Inc. 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.
+
+ This source version has been altered.
+ */
+
+#if defined(_WIN32) || defined(__i386__)
+#define B3_USE_SSE_IN_API
+#endif
+
+#include "b3Vector3.h"
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+#ifdef __APPLE__
+#include <stdint.h>
+typedef float float4 __attribute__((vector_size(16)));
+#else
+#define float4 __m128
+#endif
+//typedef  uint32_t uint4 __attribute__ ((vector_size(16)));
+
+#if defined B3_USE_SSE || defined _WIN32
+
+#define LOG2_ARRAY_SIZE 6
+#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE)
+
+#include <emmintrin.h>
+
+long b3_maxdot_large(const float *vv, const float *vec, unsigned long count, float *dotResult);
+long b3_maxdot_large(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	const float4 *vertices = (const float4 *)vv;
+	static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0};
+	float4 dotMax = b3Assign128(-B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY);
+	float4 vvec = _mm_loadu_ps(vec);
+	float4 vHi = b3CastiTo128f(_mm_shuffle_epi32(b3CastfTo128i(vvec), 0xaa));  /// zzzz
+	float4 vLo = _mm_movelh_ps(vvec, vvec);                                    /// xyxy
+
+	long maxIndex = -1L;
+
+	size_t segment = 0;
+	float4 stack_array[STACK_ARRAY_COUNT];
+
+#if DEBUG
+	// memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
+#endif
+
+	size_t index;
+	float4 max;
+	// Faster loop without cleanup code for full tiles
+	for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4)
+	{
+		max = dotMax;
+
+		for (index = 0; index < STACK_ARRAY_COUNT; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			// It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
+		}
+
+		// If we found a new max
+		if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(max, dotMax)))
+		{
+			// copy the new max across all lanes of our max accumulator
+			max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0x4e));
+			max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0xb1));
+
+			dotMax = max;
+
+			// find first occurrence of that max
+			size_t test;
+			for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++)  // local_count must be a multiple of 4
+			{
+			}
+			// record where it is.
+			maxIndex = 4 * index + segment + indexTable[test];
+		}
+	}
+
+	// account for work we've already done
+	count -= segment;
+
+	// Deal with the last < STACK_ARRAY_COUNT vectors
+	max = dotMax;
+	index = 0;
+
+	if (b3Unlikely(count > 16))
+	{
+		for (; index + 4 <= count / 4; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+
+			// It is too costly to keep the index of the max here. We will look for it again later.  We save a lot of work this way.
+		}
+	}
+
+	size_t localCount = (count & -4L) - 4 * index;
+	if (localCount)
+	{
+#ifdef __APPLE__
+		float4 t0, t1, t2, t3, t4;
+		float4 *sap = &stack_array[index + localCount / 4];
+		vertices += localCount;  // counter the offset
+		size_t byteIndex = -(localCount) * sizeof(float);
+		//AT&T Code style assembly
+		asm volatile(
+			".align 4                                                                   \n\
+             0: movaps  %[max], %[t2]                            // move max out of the way to avoid propagating NaNs in max \n\
+          movaps  (%[vertices], %[byteIndex], 4),    %[t0]    // vertices[0]      \n\
+          movaps  16(%[vertices], %[byteIndex], 4),  %[t1]    // vertices[1]      \n\
+          movaps  %[t0], %[max]                               // vertices[0]      \n\
+          movlhps %[t1], %[max]                               // x0y0x1y1         \n\
+         movaps  32(%[vertices], %[byteIndex], 4),  %[t3]    // vertices[2]      \n\
+         movaps  48(%[vertices], %[byteIndex], 4),  %[t4]    // vertices[3]      \n\
+          mulps   %[vLo], %[max]                              // x0y0x1y1 * vLo   \n\
+         movhlps %[t0], %[t1]                                // z0w0z1w1         \n\
+         movaps  %[t3], %[t0]                                // vertices[2]      \n\
+         movlhps %[t4], %[t0]                                // x2y2x3y3         \n\
+         mulps   %[vLo], %[t0]                               // x2y2x3y3 * vLo   \n\
+          movhlps %[t3], %[t4]                                // z2w2z3w3         \n\
+          shufps  $0x88, %[t4], %[t1]                         // z0z1z2z3         \n\
+          mulps   %[vHi], %[t1]                               // z0z1z2z3 * vHi   \n\
+         movaps  %[max], %[t3]                               // x0y0x1y1 * vLo   \n\
+         shufps  $0x88, %[t0], %[max]                        // x0x1x2x3 * vLo.x \n\
+         shufps  $0xdd, %[t0], %[t3]                         // y0y1y2y3 * vLo.y \n\
+         addps   %[t3], %[max]                               // x + y            \n\
+         addps   %[t1], %[max]                               // x + y + z        \n\
+         movaps  %[max], (%[sap], %[byteIndex])              // record result for later scrutiny \n\
+         maxps   %[t2], %[max]                               // record max, restore max   \n\
+         add     $16, %[byteIndex]                           // advance loop counter\n\
+         jnz     0b                                          \n\
+     "
+			: [max] "+x"(max), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
+			: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
+			: "memory", "cc");
+		index += localCount / 4;
+#else
+		{
+			for (unsigned int i = 0; i < localCount / 4; i++, index++)
+			{  // do four dot products at a time. Carefully avoid touching the w element.
+				float4 v0 = vertices[0];
+				float4 v1 = vertices[1];
+				float4 v2 = vertices[2];
+				float4 v3 = vertices[3];
+				vertices += 4;
+
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+				float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+				float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+				lo0 = lo0 * vLo;
+				lo1 = lo1 * vLo;
+				float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+				float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+				z = z * vHi;
+				x = x + y;
+				x = x + z;
+				stack_array[index] = x;
+				max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+			}
+		}
+#endif  //__APPLE__
+	}
+
+	// process the last few points
+	if (count & 3)
+	{
+		float4 v0, v1, v2, x, y, z;
+		switch (count & 3)
+		{
+			case 3:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				v2 = vertices[2];
+
+				// Calculate 3 dot products, transpose, duplicate v2
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // xyxy.lo
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z?z?.lo
+				lo0 = lo0 * vLo;
+				z = _mm_shuffle_ps(hi0, v2, 0xa8);  // z0z1z2z2
+				z = z * vHi;
+				float4 lo1 = _mm_movelh_ps(v2, v2);  // xyxy
+				lo1 = lo1 * vLo;
+				x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			}
+			break;
+			case 2:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				float4 xy = _mm_movelh_ps(v0, v1);
+				z = _mm_movehl_ps(v1, v0);
+				xy = xy * vLo;
+				z = _mm_shuffle_ps(z, z, 0xa8);
+				x = _mm_shuffle_ps(xy, xy, 0xa8);
+				y = _mm_shuffle_ps(xy, xy, 0xfd);
+				z = z * vHi;
+			}
+			break;
+			case 1:
+			{
+				float4 xy = vertices[0];
+				z = _mm_shuffle_ps(xy, xy, 0xaa);
+				xy = xy * vLo;
+				z = z * vHi;
+				x = _mm_shuffle_ps(xy, xy, 0);
+				y = _mm_shuffle_ps(xy, xy, 0x55);
+			}
+			break;
+		}
+		x = x + y;
+		x = x + z;
+		stack_array[index] = x;
+		max = _mm_max_ps(x, max);  // control the order here so that max is never NaN even if x is nan
+		index++;
+	}
+
+	// if we found a new max.
+	if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(max, dotMax)))
+	{  // we found a new max. Search for it
+		// find max across the max vector, place in all elements of max -- big latency hit here
+		max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0x4e));
+		max = _mm_max_ps(max, (float4)_mm_shuffle_ps(max, max, 0xb1));
+
+		// It is slightly faster to do this part in scalar code when count < 8. However, the common case for
+		// this where it actually makes a difference is handled in the early out at the top of the function,
+		// so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
+		// complexity, and removed it.
+
+		dotMax = max;
+
+		// scan for the first occurence of max in the array
+		size_t test;
+		for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++)  // local_count must be a multiple of 4
+		{
+		}
+		maxIndex = 4 * index + segment + indexTable[test];
+	}
+
+	_mm_store_ss(dotResult, dotMax);
+	return maxIndex;
+}
+
+long b3_mindot_large(const float *vv, const float *vec, unsigned long count, float *dotResult);
+
+long b3_mindot_large(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	const float4 *vertices = (const float4 *)vv;
+	static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0};
+
+	float4 dotmin = b3Assign128(B3_INFINITY, B3_INFINITY, B3_INFINITY, B3_INFINITY);
+	float4 vvec = _mm_loadu_ps(vec);
+	float4 vHi = b3CastiTo128f(_mm_shuffle_epi32(b3CastfTo128i(vvec), 0xaa));  /// zzzz
+	float4 vLo = _mm_movelh_ps(vvec, vvec);                                    /// xyxy
+
+	long minIndex = -1L;
+
+	size_t segment = 0;
+	float4 stack_array[STACK_ARRAY_COUNT];
+
+#if DEBUG
+	// memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) );
+#endif
+
+	size_t index;
+	float4 min;
+	// Faster loop without cleanup code for full tiles
+	for (segment = 0; segment + STACK_ARRAY_COUNT * 4 <= count; segment += STACK_ARRAY_COUNT * 4)
+	{
+		min = dotmin;
+
+		for (index = 0; index < STACK_ARRAY_COUNT; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			// It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
+		}
+
+		// If we found a new min
+		if (0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin)))
+		{
+			// copy the new min across all lanes of our min accumulator
+			min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e));
+			min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1));
+
+			dotmin = min;
+
+			// find first occurrence of that min
+			size_t test;
+			for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++)  // local_count must be a multiple of 4
+			{
+			}
+			// record where it is.
+			minIndex = 4 * index + segment + indexTable[test];
+		}
+	}
+
+	// account for work we've already done
+	count -= segment;
+
+	// Deal with the last < STACK_ARRAY_COUNT vectors
+	min = dotmin;
+	index = 0;
+
+	if (b3Unlikely(count > 16))
+	{
+		for (; index + 4 <= count / 4; index += 4)
+		{  // do four dot products at a time. Carefully avoid touching the w element.
+			float4 v0 = vertices[0];
+			float4 v1 = vertices[1];
+			float4 v2 = vertices[2];
+			float4 v3 = vertices[3];
+			vertices += 4;
+
+			float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 1] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 2] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			v0 = vertices[0];
+			v1 = vertices[1];
+			v2 = vertices[2];
+			v3 = vertices[3];
+			vertices += 4;
+
+			lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+			hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+			lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+			hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+			lo0 = lo0 * vLo;
+			lo1 = lo1 * vLo;
+			z = _mm_shuffle_ps(hi0, hi1, 0x88);
+			x = _mm_shuffle_ps(lo0, lo1, 0x88);
+			y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			z = z * vHi;
+			x = x + y;
+			x = x + z;
+			stack_array[index + 3] = x;
+			min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+
+			// It is too costly to keep the index of the min here. We will look for it again later.  We save a lot of work this way.
+		}
+	}
+
+	size_t localCount = (count & -4L) - 4 * index;
+	if (localCount)
+	{
+#ifdef __APPLE__
+		vertices += localCount;  // counter the offset
+		float4 t0, t1, t2, t3, t4;
+		size_t byteIndex = -(localCount) * sizeof(float);
+		float4 *sap = &stack_array[index + localCount / 4];
+
+		asm volatile(
+			".align 4                                                                   \n\
+             0: movaps  %[min], %[t2]                            // move min out of the way to avoid propagating NaNs in min \n\
+             movaps  (%[vertices], %[byteIndex], 4),    %[t0]    // vertices[0]      \n\
+             movaps  16(%[vertices], %[byteIndex], 4),  %[t1]    // vertices[1]      \n\
+             movaps  %[t0], %[min]                               // vertices[0]      \n\
+             movlhps %[t1], %[min]                               // x0y0x1y1         \n\
+             movaps  32(%[vertices], %[byteIndex], 4),  %[t3]    // vertices[2]      \n\
+             movaps  48(%[vertices], %[byteIndex], 4),  %[t4]    // vertices[3]      \n\
+             mulps   %[vLo], %[min]                              // x0y0x1y1 * vLo   \n\
+             movhlps %[t0], %[t1]                                // z0w0z1w1         \n\
+             movaps  %[t3], %[t0]                                // vertices[2]      \n\
+             movlhps %[t4], %[t0]                                // x2y2x3y3         \n\
+             movhlps %[t3], %[t4]                                // z2w2z3w3         \n\
+             mulps   %[vLo], %[t0]                               // x2y2x3y3 * vLo   \n\
+             shufps  $0x88, %[t4], %[t1]                         // z0z1z2z3         \n\
+             mulps   %[vHi], %[t1]                               // z0z1z2z3 * vHi   \n\
+             movaps  %[min], %[t3]                               // x0y0x1y1 * vLo   \n\
+             shufps  $0x88, %[t0], %[min]                        // x0x1x2x3 * vLo.x \n\
+             shufps  $0xdd, %[t0], %[t3]                         // y0y1y2y3 * vLo.y \n\
+             addps   %[t3], %[min]                               // x + y            \n\
+             addps   %[t1], %[min]                               // x + y + z        \n\
+             movaps  %[min], (%[sap], %[byteIndex])              // record result for later scrutiny \n\
+             minps   %[t2], %[min]                               // record min, restore min   \n\
+             add     $16, %[byteIndex]                           // advance loop counter\n\
+             jnz     0b                                          \n\
+             "
+			: [min] "+x"(min), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
+			: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
+			: "memory", "cc");
+		index += localCount / 4;
+#else
+		{
+			for (unsigned int i = 0; i < localCount / 4; i++, index++)
+			{  // do four dot products at a time. Carefully avoid touching the w element.
+				float4 v0 = vertices[0];
+				float4 v1 = vertices[1];
+				float4 v2 = vertices[2];
+				float4 v3 = vertices[3];
+				vertices += 4;
+
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // x0y0x1y1
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z0?0z1?1
+				float4 lo1 = _mm_movelh_ps(v2, v3);  // x2y2x3y3
+				float4 hi1 = _mm_movehl_ps(v3, v2);  // z2?2z3?3
+
+				lo0 = lo0 * vLo;
+				lo1 = lo1 * vLo;
+				float4 z = _mm_shuffle_ps(hi0, hi1, 0x88);
+				float4 x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+				z = z * vHi;
+				x = x + y;
+				x = x + z;
+				stack_array[index] = x;
+				min = _mm_min_ps(x, min);  // control the order here so that max is never NaN even if x is nan
+			}
+		}
+
+#endif
+	}
+
+	// process the last few points
+	if (count & 3)
+	{
+		float4 v0, v1, v2, x, y, z;
+		switch (count & 3)
+		{
+			case 3:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				v2 = vertices[2];
+
+				// Calculate 3 dot products, transpose, duplicate v2
+				float4 lo0 = _mm_movelh_ps(v0, v1);  // xyxy.lo
+				float4 hi0 = _mm_movehl_ps(v1, v0);  // z?z?.lo
+				lo0 = lo0 * vLo;
+				z = _mm_shuffle_ps(hi0, v2, 0xa8);  // z0z1z2z2
+				z = z * vHi;
+				float4 lo1 = _mm_movelh_ps(v2, v2);  // xyxy
+				lo1 = lo1 * vLo;
+				x = _mm_shuffle_ps(lo0, lo1, 0x88);
+				y = _mm_shuffle_ps(lo0, lo1, 0xdd);
+			}
+			break;
+			case 2:
+			{
+				v0 = vertices[0];
+				v1 = vertices[1];
+				float4 xy = _mm_movelh_ps(v0, v1);
+				z = _mm_movehl_ps(v1, v0);
+				xy = xy * vLo;
+				z = _mm_shuffle_ps(z, z, 0xa8);
+				x = _mm_shuffle_ps(xy, xy, 0xa8);
+				y = _mm_shuffle_ps(xy, xy, 0xfd);
+				z = z * vHi;
+			}
+			break;
+			case 1:
+			{
+				float4 xy = vertices[0];
+				z = _mm_shuffle_ps(xy, xy, 0xaa);
+				xy = xy * vLo;
+				z = z * vHi;
+				x = _mm_shuffle_ps(xy, xy, 0);
+				y = _mm_shuffle_ps(xy, xy, 0x55);
+			}
+			break;
+		}
+		x = x + y;
+		x = x + z;
+		stack_array[index] = x;
+		min = _mm_min_ps(x, min);  // control the order here so that min is never NaN even if x is nan
+		index++;
+	}
+
+	// if we found a new min.
+	if (0 == segment || 0xf != _mm_movemask_ps((float4)_mm_cmpeq_ps(min, dotmin)))
+	{  // we found a new min. Search for it
+		// find min across the min vector, place in all elements of min -- big latency hit here
+		min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0x4e));
+		min = _mm_min_ps(min, (float4)_mm_shuffle_ps(min, min, 0xb1));
+
+		// It is slightly faster to do this part in scalar code when count < 8. However, the common case for
+		// this where it actually makes a difference is handled in the early out at the top of the function,
+		// so it is less than a 1% difference here. I opted for improved code size, fewer branches and reduced
+		// complexity, and removed it.
+
+		dotmin = min;
+
+		// scan for the first occurence of min in the array
+		size_t test;
+		for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++)  // local_count must be a multiple of 4
+		{
+		}
+		minIndex = 4 * index + segment + indexTable[test];
+	}
+
+	_mm_store_ss(dotResult, dotmin);
+	return minIndex;
+}
+
+#elif defined B3_USE_NEON
+#define ARM_NEON_GCC_COMPATIBILITY 1
+#include <arm_neon.h>
+
+static long b3_maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long b3_maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long b3_maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long b3_mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long b3_mindot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult);
+static long b3_mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult);
+
+long (*b3_maxdot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = b3_maxdot_large_sel;
+long (*b3_mindot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = b3_mindot_large_sel;
+
+extern "C"
+{
+	int _get_cpu_capabilities(void);
+}
+
+static long b3_maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	if (_get_cpu_capabilities() & 0x2000)
+		b3_maxdot_large = _maxdot_large_v1;
+	else
+		b3_maxdot_large = _maxdot_large_v0;
+
+	return b3_maxdot_large(vv, vec, count, dotResult);
+}
+
+static long b3_mindot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	if (_get_cpu_capabilities() & 0x2000)
+		b3_mindot_large = _mindot_large_v1;
+	else
+		b3_mindot_large = _mindot_large_v0;
+
+	return b3_mindot_large(vv, vec, count, dotResult);
+}
+
+#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r; asm( "vld1.f32  {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+
+long b3_maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	unsigned long i = 0;
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x2_t vLo = vget_low_f32(vvec);
+	float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
+	float32x2_t dotMaxLo = (float32x2_t){-B3_INFINITY, -B3_INFINITY};
+	float32x2_t dotMaxHi = (float32x2_t){-B3_INFINITY, -B3_INFINITY};
+	uint32x2_t indexLo = (uint32x2_t){0, 1};
+	uint32x2_t indexHi = (uint32x2_t){2, 3};
+	uint32x2_t iLo = (uint32x2_t){-1, -1};
+	uint32x2_t iHi = (uint32x2_t){-1, -1};
+	const uint32x2_t four = (uint32x2_t){4, 4};
+
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+		uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		zLo = vmul_f32(z0.val[0], vHi);
+		zHi = vmul_f32(z1.val[0], vHi);
+
+		rLo = vpadd_f32(xy0, xy1);
+		rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		maskLo = vcgt_f32(rLo, dotMaxLo);
+		maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+		uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+		dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+		dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+			float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+			float32x2_t zHi = vmul_f32(vdup_lane_f32(vget_high_f32(v2), 0), vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			float32x2_t rHi = vpadd_f32(xy2, xy2);
+			rLo = vadd_f32(rLo, zLo);
+			rHi = vadd_f32(rHi, zHi);
+
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			uint32x2_t maskHi = vcgt_f32(rHi, dotMaxHi);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			dotMaxHi = vbsl_f32(maskHi, rHi, dotMaxHi);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+			iHi = vbsl_u32(maskHi, indexHi, iHi);
+		}
+		break;
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			rLo = vadd_f32(rLo, zLo);
+
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
+			float32x2_t zLo = vmul_f32(z0, vHi);
+			float32x2_t rLo = vpadd_f32(xy0, xy0);
+			rLo = vadd_f32(rLo, zLo);
+			uint32x2_t maskLo = vcgt_f32(rLo, dotMaxLo);
+			dotMaxLo = vbsl_f32(maskLo, rLo, dotMaxLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vcgt_f32(dotMaxHi, dotMaxLo);
+	dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	// select best answer between even and odd results
+	dotMaxHi = vdup_lane_f32(dotMaxLo, 1);
+	iHi = vdup_lane_u32(iLo, 1);
+	mask = vcgt_f32(dotMaxHi, dotMaxLo);
+	dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	*dotResult = vget_lane_f32(dotMaxLo, 0);
+	return vget_lane_u32(iLo, 0);
+}
+
+long b3_maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
+	float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
+	const uint32x4_t four = (uint32x4_t){4, 4, 4, 4};
+	uint32x4_t local_index = (uint32x4_t){0, 1, 2, 3};
+	uint32x4_t index = (uint32x4_t){-1, -1, -1, -1};
+	float32x4_t maxDot = (float32x4_t){-B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY};
+
+	unsigned long i = 0;
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		zb = vuzpq_f32(z0, z1);
+		z = vmulq_f32(zb.val[0], vHi);
+		xy = vuzpq_f32(xy0, xy1);
+		x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcgtq_f32(x, maxDot);
+		maxDot = vbslq_f32(mask, x, maxDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v2));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v2));
+
+			xy0 = vmulq_f32(xy0, vLo);
+			xy1 = vmulq_f32(xy1, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z1);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z0);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v0));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			z = vmulq_f32(z, vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcgtq_f32(x, maxDot);
+			maxDot = vbslq_f32(mask, x, maxDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vcgt_f32(vget_high_f32(maxDot), vget_low_f32(maxDot));
+	float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot));
+	uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
+
+	// select best answer between even and odd results
+	float32x2_t maxDotO = vdup_lane_f32(maxDot2, 1);
+	uint32x2_t indexHi = vdup_lane_u32(index2, 1);
+	mask = vcgt_f32(maxDotO, maxDot2);
+	maxDot2 = vbsl_f32(mask, maxDotO, maxDot2);
+	index2 = vbsl_u32(mask, indexHi, index2);
+
+	*dotResult = vget_lane_f32(maxDot2, 0);
+	return vget_lane_u32(index2, 0);
+}
+
+long b3_mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	unsigned long i = 0;
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x2_t vLo = vget_low_f32(vvec);
+	float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
+	float32x2_t dotMinLo = (float32x2_t){B3_INFINITY, B3_INFINITY};
+	float32x2_t dotMinHi = (float32x2_t){B3_INFINITY, B3_INFINITY};
+	uint32x2_t indexLo = (uint32x2_t){0, 1};
+	uint32x2_t indexHi = (uint32x2_t){2, 3};
+	uint32x2_t iLo = (uint32x2_t){-1, -1};
+	uint32x2_t iHi = (uint32x2_t){-1, -1};
+	const uint32x2_t four = (uint32x2_t){4, 4};
+
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+		uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		zLo = vmul_f32(z0.val[0], vHi);
+		zHi = vmul_f32(z1.val[0], vHi);
+
+		rLo = vpadd_f32(xy0, xy1);
+		rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		maskLo = vclt_f32(rLo, dotMinLo);
+		maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+		float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+		float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+		float32x2_t xy3 = vmul_f32(vget_low_f32(v3), vLo);
+
+		float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x2x2_t z1 = vtrn_f32(vget_high_f32(v2), vget_high_f32(v3));
+		float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+		float32x2_t zHi = vmul_f32(z1.val[0], vHi);
+
+		float32x2_t rLo = vpadd_f32(xy0, xy1);
+		float32x2_t rHi = vpadd_f32(xy2, xy3);
+		rLo = vadd_f32(rLo, zLo);
+		rHi = vadd_f32(rHi, zHi);
+
+		uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+		uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+		dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+		dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+		iLo = vbsl_u32(maskLo, indexLo, iLo);
+		iHi = vbsl_u32(maskHi, indexHi, iHi);
+		indexLo = vadd_u32(indexLo, four);
+		indexHi = vadd_u32(indexHi, four);
+	}
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+			float32x2_t xy2 = vmul_f32(vget_low_f32(v2), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+			float32x2_t zHi = vmul_f32(vdup_lane_f32(vget_high_f32(v2), 0), vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			float32x2_t rHi = vpadd_f32(xy2, xy2);
+			rLo = vadd_f32(rLo, zLo);
+			rHi = vadd_f32(rHi, zHi);
+
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			uint32x2_t maskHi = vclt_f32(rHi, dotMinHi);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			dotMinHi = vbsl_f32(maskHi, rHi, dotMinHi);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+			iHi = vbsl_u32(maskHi, indexHi, iHi);
+		}
+		break;
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t xy1 = vmul_f32(vget_low_f32(v1), vLo);
+
+			float32x2x2_t z0 = vtrn_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x2_t zLo = vmul_f32(z0.val[0], vHi);
+
+			float32x2_t rLo = vpadd_f32(xy0, xy1);
+			rLo = vadd_f32(rLo, zLo);
+
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x2_t xy0 = vmul_f32(vget_low_f32(v0), vLo);
+			float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0);
+			float32x2_t zLo = vmul_f32(z0, vHi);
+			float32x2_t rLo = vpadd_f32(xy0, xy0);
+			rLo = vadd_f32(rLo, zLo);
+			uint32x2_t maskLo = vclt_f32(rLo, dotMinLo);
+			dotMinLo = vbsl_f32(maskLo, rLo, dotMinLo);
+			iLo = vbsl_u32(maskLo, indexLo, iLo);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vclt_f32(dotMinHi, dotMinLo);
+	dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	// select best answer between even and odd results
+	dotMinHi = vdup_lane_f32(dotMinLo, 1);
+	iHi = vdup_lane_u32(iLo, 1);
+	mask = vclt_f32(dotMinHi, dotMinLo);
+	dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo);
+	iLo = vbsl_u32(mask, iHi, iLo);
+
+	*dotResult = vget_lane_f32(dotMinLo, 0);
+	return vget_lane_u32(iLo, 0);
+}
+
+long b3_mindot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult)
+{
+	float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
+	float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec));
+	float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0);
+	const uint32x4_t four = (uint32x4_t){4, 4, 4, 4};
+	uint32x4_t local_index = (uint32x4_t){0, 1, 2, 3};
+	uint32x4_t index = (uint32x4_t){-1, -1, -1, -1};
+	float32x4_t minDot = (float32x4_t){B3_INFINITY, B3_INFINITY, B3_INFINITY, B3_INFINITY};
+
+	unsigned long i = 0;
+	for (; i + 8 <= count; i += 8)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+
+		v0 = vld1q_f32_aligned_postincrement(vv);
+		v1 = vld1q_f32_aligned_postincrement(vv);
+		v2 = vld1q_f32_aligned_postincrement(vv);
+		v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		zb = vuzpq_f32(z0, z1);
+		z = vmulq_f32(zb.val[0], vHi);
+		xy = vuzpq_f32(xy0, xy1);
+		x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	for (; i + 4 <= count; i += 4)
+	{
+		float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+		float32x4_t v3 = vld1q_f32_aligned_postincrement(vv);
+
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+		float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v3));
+		// the next two lines should resolve to a single vswp d, d
+		float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+		float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v3));
+
+		xy0 = vmulq_f32(xy0, vLo);
+		xy1 = vmulq_f32(xy1, vLo);
+
+		float32x4x2_t zb = vuzpq_f32(z0, z1);
+		float32x4_t z = vmulq_f32(zb.val[0], vHi);
+		float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+		float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+		x = vaddq_f32(x, z);
+
+		uint32x4_t mask = vcltq_f32(x, minDot);
+		minDot = vbslq_f32(mask, x, minDot);
+		index = vbslq_u32(mask, local_index, index);
+		local_index = vaddq_u32(local_index, four);
+	}
+
+	switch (count & 3)
+	{
+		case 3:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v2 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			float32x4_t xy1 = vcombine_f32(vget_low_f32(v2), vget_low_f32(v2));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+			float32x4_t z1 = vcombine_f32(vget_high_f32(v2), vget_high_f32(v2));
+
+			xy0 = vmulq_f32(xy0, vLo);
+			xy1 = vmulq_f32(xy1, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z1);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy1);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 2:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+			float32x4_t v1 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v1));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z0 = vcombine_f32(vget_high_f32(v0), vget_high_f32(v1));
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			float32x4x2_t zb = vuzpq_f32(z0, z0);
+			float32x4_t z = vmulq_f32(zb.val[0], vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		case 1:
+		{
+			float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
+
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t xy0 = vcombine_f32(vget_low_f32(v0), vget_low_f32(v0));
+			// the next two lines should resolve to a single vswp d, d
+			float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0);
+
+			xy0 = vmulq_f32(xy0, vLo);
+
+			z = vmulq_f32(z, vHi);
+			float32x4x2_t xy = vuzpq_f32(xy0, xy0);
+			float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]);
+			x = vaddq_f32(x, z);
+
+			uint32x4_t mask = vcltq_f32(x, minDot);
+			minDot = vbslq_f32(mask, x, minDot);
+			index = vbslq_u32(mask, local_index, index);
+			local_index = vaddq_u32(local_index, four);
+		}
+		break;
+
+		default:
+			break;
+	}
+
+	// select best answer between hi and lo results
+	uint32x2_t mask = vclt_f32(vget_high_f32(minDot), vget_low_f32(minDot));
+	float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot));
+	uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index));
+
+	// select best answer between even and odd results
+	float32x2_t minDotO = vdup_lane_f32(minDot2, 1);
+	uint32x2_t indexHi = vdup_lane_u32(index2, 1);
+	mask = vclt_f32(minDotO, minDot2);
+	minDot2 = vbsl_f32(mask, minDotO, minDot2);
+	index2 = vbsl_u32(mask, indexHi, index2);
+
+	*dotResult = vget_lane_f32(minDot2, 0);
+	return vget_lane_u32(index2, 0);
+}
+
+#else
+#error Unhandled __APPLE__ arch
+#endif
+
+#endif /* __APPLE__ */

Engine/lib/bullet/src/Bullet3Common/b3Vector3.h

@@ -0,0 +1,1303 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / 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 B3_VECTOR3_H
+#define B3_VECTOR3_H
+
+//#include <stdint.h>
+#include "b3Scalar.h"
+#include "b3MinMax.h"
+#include "b3AlignedAllocator.h"
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3Vector3Data b3Vector3DoubleData
+#define b3Vector3DataName "b3Vector3DoubleData"
+#else
+#define b3Vector3Data b3Vector3FloatData
+#define b3Vector3DataName "b3Vector3FloatData"
+#endif  //B3_USE_DOUBLE_PRECISION
+
+#if defined B3_USE_SSE
+
+//typedef  uint32_t __m128i __attribute__ ((vector_size(16)));
+
+#ifdef _MSC_VER
+#pragma warning(disable : 4556)  // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
+#endif
+
+#define B3_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
+//#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
+#define b3_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
+#define b3_splat3_ps(_a, _i) b3_pshufd_ps((_a), B3_SHUFFLE(_i, _i, _i, 3))
+#define b3_splat_ps(_a, _i) b3_pshufd_ps((_a), B3_SHUFFLE(_i, _i, _i, _i))
+
+#define b3v3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
+#define b3vAbsMask (_mm_set_epi32(0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
+#define b3vFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
+#define b3v3AbsfMask b3CastiTo128f(b3v3AbsiMask)
+#define b3vFFF0fMask b3CastiTo128f(b3vFFF0Mask)
+#define b3vxyzMaskf b3vFFF0fMask
+#define b3vAbsfMask b3CastiTo128f(b3vAbsMask)
+
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1_5) = {1.5f, 1.5f, 1.5f, 1.5f};
+
+#endif
+
+#ifdef B3_USE_NEON
+
+const float32x4_t B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
+const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3v3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
+
+#endif
+
+class b3Vector3;
+class b3Vector4;
+
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+//#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
+inline b3Vector3 b3MakeVector3(b3SimdFloat4 v);
+inline b3Vector4 b3MakeVector4(b3SimdFloat4 vec);
+#endif
+
+inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z);
+inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w);
+inline b3Vector4 b3MakeVector4(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w);
+
+/**@brief b3Vector3 can be used to represent 3D points and vectors.
+ * It has an un-used w component to suit 16-byte alignment when b3Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+ * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+ */
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Vector3
+{
+public:
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)  // _WIN32 || ARM
+	union {
+		b3SimdFloat4 mVec128;
+		float m_floats[4];
+		struct
+		{
+			float x, y, z, w;
+		};
+	};
+#else
+	union {
+		float m_floats[4];
+		struct
+		{
+			float x, y, z, w;
+		};
+	};
+#endif
+
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)  // _WIN32 || ARM
+
+	/*B3_FORCE_INLINE		b3Vector3()
+	{
+	}
+	*/
+
+	B3_FORCE_INLINE b3SimdFloat4 get128() const
+	{
+		return mVec128;
+	}
+	B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
+	{
+		mVec128 = v128;
+	}
+#endif
+
+public:
+	/**@brief Add a vector to this one
+ * @param The vector to add to this one */
+	B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_add_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vaddq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] += v.m_floats[0];
+		m_floats[1] += v.m_floats[1];
+		m_floats[2] += v.m_floats[2];
+#endif
+		return *this;
+	}
+
+	/**@brief Subtract a vector from this one
+   * @param The vector to subtract */
+	B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_sub_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vsubq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] -= v.m_floats[0];
+		m_floats[1] -= v.m_floats[1];
+		m_floats[2] -= v.m_floats[2];
+#endif
+		return *this;
+	}
+
+	/**@brief Scale the vector
+   * @param s Scale factor */
+	B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
+		mVec128 = _mm_mul_ps(mVec128, vs);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_n_f32(mVec128, s);
+#else
+		m_floats[0] *= s;
+		m_floats[1] *= s;
+		m_floats[2] *= s;
+#endif
+		return *this;
+	}
+
+	/**@brief Inversely scale the vector
+   * @param s Scale factor to divide by */
+	B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
+	{
+		b3FullAssert(s != b3Scalar(0.0));
+
+#if 0  //defined(B3_USE_SSE_IN_API)
+// this code is not faster !
+		__m128 vs = _mm_load_ss(&s);
+		vs = _mm_div_ss(b3v1110, vs);
+		vs = b3_pshufd_ps(vs, 0x00);	//	(S S S S)
+
+		mVec128 = _mm_mul_ps(mVec128, vs);
+
+		return *this;
+#else
+		return *this *= b3Scalar(1.0) / s;
+#endif
+	}
+
+	/**@brief Return the dot product
+   * @param v The other vector in the dot product */
+	B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
+		__m128 z = _mm_movehl_ps(vd, vd);
+		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, y);
+		vd = _mm_add_ss(vd, z);
+		return _mm_cvtss_f32(vd);
+#elif defined(B3_USE_NEON)
+		float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
+		x = vadd_f32(x, vget_high_f32(vd));
+		return vget_lane_f32(x, 0);
+#else
+		return m_floats[0] * v.m_floats[0] +
+			   m_floats[1] * v.m_floats[1] +
+			   m_floats[2] * v.m_floats[2];
+#endif
+	}
+
+	/**@brief Return the length of the vector squared */
+	B3_FORCE_INLINE b3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+	/**@brief Return the length of the vector */
+	B3_FORCE_INLINE b3Scalar length() const
+	{
+		return b3Sqrt(length2());
+	}
+
+	/**@brief Return the distance squared between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	B3_FORCE_INLINE b3Scalar distance2(const b3Vector3& v) const;
+
+	/**@brief Return the distance between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const;
+
+	B3_FORCE_INLINE b3Vector3& safeNormalize()
+	{
+		b3Scalar l2 = length2();
+		//triNormal.normalize();
+		if (l2 >= B3_EPSILON * B3_EPSILON)
+		{
+			(*this) /= b3Sqrt(l2);
+		}
+		else
+		{
+			setValue(1, 0, 0);
+		}
+		return *this;
+	}
+
+	/**@brief Normalize this vector
+   * x^2 + y^2 + z^2 = 1 */
+	B3_FORCE_INLINE b3Vector3& normalize()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		// dot product first
+		__m128 vd = _mm_mul_ps(mVec128, mVec128);
+		__m128 z = _mm_movehl_ps(vd, vd);
+		__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, y);
+		vd = _mm_add_ss(vd, z);
+
+#if 0
+        vd = _mm_sqrt_ss(vd);
+		vd = _mm_div_ss(b3v1110, vd);
+		vd = b3_splat_ps(vd, 0x80);
+		mVec128 = _mm_mul_ps(mVec128, vd);
+#else
+
+		// NR step 1/sqrt(x) - vd is x, y is output
+		y = _mm_rsqrt_ss(vd);  // estimate
+
+		//  one step NR
+		z = b3v1_5;
+		vd = _mm_mul_ss(vd, b3vHalf);  // vd * 0.5
+		//x2 = vd;
+		vd = _mm_mul_ss(vd, y);  // vd * 0.5 * y0
+		vd = _mm_mul_ss(vd, y);  // vd * 0.5 * y0 * y0
+		z = _mm_sub_ss(z, vd);   // 1.5 - vd * 0.5 * y0 * y0
+
+		y = _mm_mul_ss(y, z);  // y0 * (1.5 - vd * 0.5 * y0 * y0)
+
+		y = b3_splat_ps(y, 0x80);
+		mVec128 = _mm_mul_ps(mVec128, y);
+
+#endif
+
+		return *this;
+#else
+		return *this /= length();
+#endif
+	}
+
+	/**@brief Return a normalized version of this vector */
+	B3_FORCE_INLINE b3Vector3 normalized() const;
+
+	/**@brief Return a rotated version of this vector
+   * @param wAxis The axis to rotate about
+   * @param angle The angle to rotate by */
+	B3_FORCE_INLINE b3Vector3 rotate(const b3Vector3& wAxis, const b3Scalar angle) const;
+
+	/**@brief Return the angle between this and another vector
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
+	{
+		b3Scalar s = b3Sqrt(length2() * v.length2());
+		b3FullAssert(s != b3Scalar(0.0));
+		return b3Acos(dot(v) / s);
+	}
+
+	/**@brief Return a vector will the absolute values of each element */
+	B3_FORCE_INLINE b3Vector3 absolute() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask));
+#elif defined(B3_USE_NEON)
+		return b3Vector3(vabsq_f32(mVec128));
+#else
+		return b3MakeVector3(
+			b3Fabs(m_floats[0]),
+			b3Fabs(m_floats[1]),
+			b3Fabs(m_floats[2]));
+#endif
+	}
+
+	/**@brief Return the cross product between this and another vector
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 T, V;
+
+		T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3));    //	(Y Z X 0)
+		V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+
+		V = _mm_mul_ps(V, mVec128);
+		T = _mm_mul_ps(T, v.mVec128);
+		V = _mm_sub_ps(V, T);
+
+		V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3));
+		return b3MakeVector3(V);
+#elif defined(B3_USE_NEON)
+		float32x4_t T, V;
+		// form (Y, Z, X, _) of mVec128 and v.mVec128
+		float32x2_t Tlow = vget_low_f32(mVec128);
+		float32x2_t Vlow = vget_low_f32(v.mVec128);
+		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
+
+		V = vmulq_f32(V, mVec128);
+		T = vmulq_f32(T, v.mVec128);
+		V = vsubq_f32(V, T);
+		Vlow = vget_low_f32(V);
+		// form (Y, Z, X, _);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
+		V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask);
+
+		return b3Vector3(V);
+#else
+		return b3MakeVector3(
+			m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+#endif
+	}
+
+	B3_FORCE_INLINE b3Scalar triple(const b3Vector3& v1, const b3Vector3& v2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		// cross:
+		__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+		__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3));  //	(Y Z X 0)
+
+		V = _mm_mul_ps(V, v1.mVec128);
+		T = _mm_mul_ps(T, v2.mVec128);
+		V = _mm_sub_ps(V, T);
+
+		V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3));
+
+		// dot:
+		V = _mm_mul_ps(V, mVec128);
+		__m128 z = _mm_movehl_ps(V, V);
+		__m128 y = _mm_shuffle_ps(V, V, 0x55);
+		V = _mm_add_ss(V, y);
+		V = _mm_add_ss(V, z);
+		return _mm_cvtss_f32(V);
+
+#elif defined(B3_USE_NEON)
+		// cross:
+		float32x4_t T, V;
+		// form (Y, Z, X, _) of mVec128 and v.mVec128
+		float32x2_t Tlow = vget_low_f32(v1.mVec128);
+		float32x2_t Vlow = vget_low_f32(v2.mVec128);
+		T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
+
+		V = vmulq_f32(V, v1.mVec128);
+		T = vmulq_f32(T, v2.mVec128);
+		V = vsubq_f32(V, T);
+		Vlow = vget_low_f32(V);
+		// form (Y, Z, X, _);
+		V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
+
+		// dot:
+		V = vmulq_f32(mVec128, V);
+		float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));
+		x = vadd_f32(x, vget_high_f32(V));
+		return vget_lane_f32(x, 0);
+#else
+		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
+			   m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
+			   m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+#endif
+	}
+
+	/**@brief Return the axis with the smallest value
+   * Note return values are 0,1,2 for x, y, or z */
+	B3_FORCE_INLINE int minAxis() const
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+	}
+
+	/**@brief Return the axis with the largest value
+   * Note return values are 0,1,2 for x, y, or z */
+	B3_FORCE_INLINE int maxAxis() const
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+	}
+
+	B3_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	B3_FORCE_INLINE int closestAxis() const
+	{
+		return absolute().maxAxis();
+	}
+
+	B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vrt = _mm_load_ss(&rt);  //	(rt 0 0 0)
+		b3Scalar s = b3Scalar(1.0) - rt;
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
+		__m128 r0 = _mm_mul_ps(v0.mVec128, vs);
+		vrt = b3_pshufd_ps(vrt, 0x80);  //	(rt rt rt 0.0)
+		__m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
+		__m128 tmp3 = _mm_add_ps(r0, r1);
+		mVec128 = tmp3;
+#elif defined(B3_USE_NEON)
+		float32x4_t vl = vsubq_f32(v1.mVec128, v0.mVec128);
+		vl = vmulq_n_f32(vl, rt);
+		mVec128 = vaddq_f32(vl, v0.mVec128);
+#else
+		b3Scalar s = b3Scalar(1.0) - rt;
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+#endif
+	}
+
+	/**@brief Return the linear interpolation between this and another vector
+   * @param v The other vector
+   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
+	B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vt = _mm_load_ss(&t);  //	(t 0 0 0)
+		vt = b3_pshufd_ps(vt, 0x80);  //	(rt rt rt 0.0)
+		__m128 vl = _mm_sub_ps(v.mVec128, mVec128);
+		vl = _mm_mul_ps(vl, vt);
+		vl = _mm_add_ps(vl, mVec128);
+
+		return b3MakeVector3(vl);
+#elif defined(B3_USE_NEON)
+		float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
+		vl = vmulq_n_f32(vl, t);
+		vl = vaddq_f32(vl, mVec128);
+
+		return b3Vector3(vl);
+#else
+		return b3MakeVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+							 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+							 m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
+#endif
+	}
+
+	/**@brief Elementwise multiply this vector by the other
+   * @param v The other vector */
+	B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_mul_ps(mVec128, v.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_f32(mVec128, v.mVec128);
+#else
+		m_floats[0] *= v.m_floats[0];
+		m_floats[1] *= v.m_floats[1];
+		m_floats[2] *= v.m_floats[2];
+#endif
+		return *this;
+	}
+
+	/**@brief Return the x value */
+	B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+	/**@brief Return the w value */
+	B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
+
+	/**@brief Set the x value */
+	B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
+	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
+	B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
+
+	B3_FORCE_INLINE bool operator==(const b3Vector3& other) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3] == other.m_floats[3]) &&
+				(m_floats[2] == other.m_floats[2]) &&
+				(m_floats[1] == other.m_floats[1]) &&
+				(m_floats[0] == other.m_floats[0]));
+#endif
+	}
+
+	B3_FORCE_INLINE bool operator!=(const b3Vector3& other) const
+	{
+		return !(*this == other);
+	}
+
+	/**@brief Set each element to the max of the current values and the values of another b3Vector3
+   * @param other The other b3Vector3 to compare with
+   */
+	B3_FORCE_INLINE void setMax(const b3Vector3& other)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		b3SetMax(m_floats[0], other.m_floats[0]);
+		b3SetMax(m_floats[1], other.m_floats[1]);
+		b3SetMax(m_floats[2], other.m_floats[2]);
+		b3SetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+
+	/**@brief Set each element to the min of the current values and the values of another b3Vector3
+   * @param other The other b3Vector3 to compare with
+   */
+	B3_FORCE_INLINE void setMin(const b3Vector3& other)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		b3SetMin(m_floats[0], other.m_floats[0]);
+		b3SetMin(m_floats[1], other.m_floats[1]);
+		b3SetMin(m_floats[2], other.m_floats[2]);
+		b3SetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
+
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = b3Scalar(0.f);
+	}
+
+	void getSkewSymmetricMatrix(b3Vector3 * v0, b3Vector3 * v1, b3Vector3 * v2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+
+		__m128 V = _mm_and_ps(mVec128, b3vFFF0fMask);
+		__m128 V0 = _mm_xor_ps(b3vMzeroMask, V);
+		__m128 V2 = _mm_movelh_ps(V0, V);
+
+		__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
+
+		V0 = _mm_shuffle_ps(V0, V, 0xDB);
+		V2 = _mm_shuffle_ps(V2, V, 0xF9);
+
+		v0->mVec128 = V0;
+		v1->mVec128 = V1;
+		v2->mVec128 = V2;
+#else
+		v0->setValue(0., -getZ(), getY());
+		v1->setValue(getZ(), 0., -getX());
+		v2->setValue(-getY(), getX(), 0.);
+#endif
+	}
+
+	void setZero()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
+#elif defined(B3_USE_NEON)
+		int32x4_t vi = vdupq_n_s32(0);
+		mVec128 = vreinterpretq_f32_s32(vi);
+#else
+		setValue(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
+#endif
+	}
+
+	B3_FORCE_INLINE bool isZero() const
+	{
+		return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0);
+	}
+
+	B3_FORCE_INLINE bool fuzzyZero() const
+	{
+		return length2() < B3_EPSILON;
+	}
+
+	B3_FORCE_INLINE void serialize(struct b3Vector3Data & dataOut) const;
+
+	B3_FORCE_INLINE void deSerialize(const struct b3Vector3Data& dataIn);
+
+	B3_FORCE_INLINE void serializeFloat(struct b3Vector3FloatData & dataOut) const;
+
+	B3_FORCE_INLINE void deSerializeFloat(const struct b3Vector3FloatData& dataIn);
+
+	B3_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData & dataOut) const;
+
+	B3_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn);
+
+	/**@brief returns index of maximum dot product between this and vectors in array[]
+         * @param array The other vectors
+         * @param array_count The number of other vectors
+         * @param dotOut The maximum dot product */
+	B3_FORCE_INLINE long maxDot(const b3Vector3* array, long array_count, b3Scalar& dotOut) const;
+
+	/**@brief returns index of minimum dot product between this and vectors in array[]
+         * @param array The other vectors
+         * @param array_count The number of other vectors
+         * @param dotOut The minimum dot product */
+	B3_FORCE_INLINE long minDot(const b3Vector3* array, long array_count, b3Scalar& dotOut) const;
+
+	/* create a vector as  b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 ))  */
+	B3_FORCE_INLINE b3Vector3 dot3(const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+
+		__m128 a0 = _mm_mul_ps(v0.mVec128, this->mVec128);
+		__m128 a1 = _mm_mul_ps(v1.mVec128, this->mVec128);
+		__m128 a2 = _mm_mul_ps(v2.mVec128, this->mVec128);
+		__m128 b0 = _mm_unpacklo_ps(a0, a1);
+		__m128 b1 = _mm_unpackhi_ps(a0, a1);
+		__m128 b2 = _mm_unpacklo_ps(a2, _mm_setzero_ps());
+		__m128 r = _mm_movelh_ps(b0, b2);
+		r = _mm_add_ps(r, _mm_movehl_ps(b2, b0));
+		a2 = _mm_and_ps(a2, b3vxyzMaskf);
+		r = _mm_add_ps(r, b3CastdTo128f(_mm_move_sd(b3CastfTo128d(a2), b3CastfTo128d(b1))));
+		return b3MakeVector3(r);
+
+#elif defined(B3_USE_NEON)
+		static const uint32x4_t xyzMask = (const uint32x4_t){-1, -1, -1, 0};
+		float32x4_t a0 = vmulq_f32(v0.mVec128, this->mVec128);
+		float32x4_t a1 = vmulq_f32(v1.mVec128, this->mVec128);
+		float32x4_t a2 = vmulq_f32(v2.mVec128, this->mVec128);
+		float32x2x2_t zLo = vtrn_f32(vget_high_f32(a0), vget_high_f32(a1));
+		a2 = (float32x4_t)vandq_u32((uint32x4_t)a2, xyzMask);
+		float32x2_t b0 = vadd_f32(vpadd_f32(vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0]);
+		float32x2_t b1 = vpadd_f32(vpadd_f32(vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
+		return b3Vector3(vcombine_f32(b0, b1));
+#else
+		return b3MakeVector3(dot(v0), dot(v1), dot(v2));
+#endif
+	}
+};
+
+/**@brief Return the sum of two vectors (Point symantics)*/
+B3_FORCE_INLINE b3Vector3
+operator+(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128));
+#else
+	return b3MakeVector3(
+		v1.m_floats[0] + v2.m_floats[0],
+		v1.m_floats[1] + v2.m_floats[1],
+		v1.m_floats[2] + v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the elementwise product of two vectors */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128));
+#else
+	return b3MakeVector3(
+		v1.m_floats[0] * v2.m_floats[0],
+		v1.m_floats[1] * v2.m_floats[1],
+		v1.m_floats[2] * v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the difference between two vectors */
+B3_FORCE_INLINE b3Vector3
+operator-(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+
+	//	without _mm_and_ps this code causes slowdown in Concave moving
+	__m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
+	return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
+#else
+	return b3MakeVector3(
+		v1.m_floats[0] - v2.m_floats[0],
+		v1.m_floats[1] - v2.m_floats[1],
+		v1.m_floats[2] - v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the negative of the vector */
+B3_FORCE_INLINE b3Vector3
+operator-(const b3Vector3& v)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	__m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask);
+	return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask));
+#else
+	return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+#endif
+}
+
+/**@brief Return the vector scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Vector3& v, const b3Scalar& s)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+	vs = b3_pshufd_ps(vs, 0x80);  //	(S S S 0.0)
+	return b3MakeVector3(_mm_mul_ps(v.mVec128, vs));
+#elif defined(B3_USE_NEON)
+	float32x4_t r = vmulq_n_f32(v.mVec128, s);
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
+#else
+	return b3MakeVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+#endif
+}
+
+/**@brief Return the vector scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator*(const b3Scalar& s, const b3Vector3& v)
+{
+	return v * s;
+}
+
+/**@brief Return the vector inversely scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator/(const b3Vector3& v, const b3Scalar& s)
+{
+	b3FullAssert(s != b3Scalar(0.0));
+#if 0  //defined(B3_USE_SSE_IN_API)
+// this code is not faster !
+	__m128 vs = _mm_load_ss(&s);
+    vs = _mm_div_ss(b3v1110, vs);
+	vs = b3_pshufd_ps(vs, 0x00);	//	(S S S S)
+
+	return b3Vector3(_mm_mul_ps(v.mVec128, vs));
+#else
+	return v * (b3Scalar(1.0) / s);
+#endif
+}
+
+/**@brief Return the vector inversely scaled by s */
+B3_FORCE_INLINE b3Vector3
+operator/(const b3Vector3& v1, const b3Vector3& v2)
+{
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
+	__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
+	vec = _mm_and_ps(vec, b3vFFF0fMask);
+	return b3MakeVector3(vec);
+#elif defined(B3_USE_NEON)
+	float32x4_t x, y, v, m;
+
+	x = v1.mVec128;
+	y = v2.mVec128;
+
+	v = vrecpeq_f32(y);     // v ~ 1/y
+	m = vrecpsq_f32(y, v);  // m = (2-v*y)
+	v = vmulq_f32(v, m);    // vv = v*m ~~ 1/y
+	m = vrecpsq_f32(y, v);  // mm = (2-vv*y)
+	v = vmulq_f32(v, x);    // x*vv
+	v = vmulq_f32(v, m);    // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
+
+	return b3Vector3(v);
+#else
+	return b3MakeVector3(
+		v1.m_floats[0] / v2.m_floats[0],
+		v1.m_floats[1] / v2.m_floats[1],
+		v1.m_floats[2] / v2.m_floats[2]);
+#endif
+}
+
+/**@brief Return the dot product between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Dot(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.dot(v2);
+}
+
+/**@brief Return the distance squared between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.distance2(v2);
+}
+
+/**@brief Return the distance between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Distance(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.distance(v2);
+}
+
+/**@brief Return the angle between two vectors */
+B3_FORCE_INLINE b3Scalar
+b3Angle(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.angle(v2);
+}
+
+/**@brief Return the cross product of two vectors */
+B3_FORCE_INLINE b3Vector3
+b3Cross(const b3Vector3& v1, const b3Vector3& v2)
+{
+	return v1.cross(v2);
+}
+
+B3_FORCE_INLINE b3Scalar
+b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+/**@brief Return the linear interpolation between two vectors
+ * @param v1 One vector
+ * @param v2 The other vector
+ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
+B3_FORCE_INLINE b3Vector3
+b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+B3_FORCE_INLINE b3Scalar b3Vector3::distance2(const b3Vector3& v) const
+{
+	return (v - *this).length2();
+}
+
+B3_FORCE_INLINE b3Scalar b3Vector3::distance(const b3Vector3& v) const
+{
+	return (v - *this).length();
+}
+
+B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	b3Vector3 norm = *this;
+
+	return norm.normalize();
+#else
+	return *this / length();
+#endif
+}
+
+B3_FORCE_INLINE b3Vector3 b3Vector3::rotate(const b3Vector3& wAxis, const b3Scalar _angle) const
+{
+	// wAxis must be a unit lenght vector
+
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+
+	__m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
+	b3Scalar ssin = b3Sin(_angle);
+	__m128 C = wAxis.cross(b3MakeVector3(mVec128)).mVec128;
+	O = _mm_and_ps(O, b3vFFF0fMask);
+	b3Scalar scos = b3Cos(_angle);
+
+	__m128 vsin = _mm_load_ss(&ssin);  //	(S 0 0 0)
+	__m128 vcos = _mm_load_ss(&scos);  //	(S 0 0 0)
+
+	__m128 Y = b3_pshufd_ps(O, 0xC9);  //	(Y Z X 0)
+	__m128 Z = b3_pshufd_ps(O, 0xD2);  //	(Z X Y 0)
+	O = _mm_add_ps(O, Y);
+	vsin = b3_pshufd_ps(vsin, 0x80);  //	(S S S 0)
+	O = _mm_add_ps(O, Z);
+	vcos = b3_pshufd_ps(vcos, 0x80);  //	(S S S 0)
+
+	vsin = vsin * C;
+	O = O * wAxis.mVec128;
+	__m128 X = mVec128 - O;
+
+	O = O + vsin;
+	vcos = vcos * X;
+	O = O + vcos;
+
+	return b3MakeVector3(O);
+#else
+	b3Vector3 o = wAxis * wAxis.dot(*this);
+	b3Vector3 _x = *this - o;
+	b3Vector3 _y;
+
+	_y = wAxis.cross(*this);
+
+	return (o + _x * b3Cos(_angle) + _y * b3Sin(_angle));
+#endif
+}
+
+B3_FORCE_INLINE long b3Vector3::maxDot(const b3Vector3* array, long array_count, b3Scalar& dotOut) const
+{
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+#if defined _WIN32 || defined(B3_USE_SSE)
+	const long scalar_cutoff = 10;
+	long b3_maxdot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#elif defined B3_USE_NEON
+	const long scalar_cutoff = 4;
+	extern long (*_maxdot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#endif
+	if (array_count < scalar_cutoff)
+#else
+
+#endif  //B3_USE_SSE || B3_USE_NEON
+	{
+		b3Scalar maxDot = -B3_INFINITY;
+		int i = 0;
+		int ptIndex = -1;
+		for (i = 0; i < array_count; i++)
+		{
+			b3Scalar dot = array[i].dot(*this);
+
+			if (dot > maxDot)
+			{
+				maxDot = dot;
+				ptIndex = i;
+			}
+		}
+
+		b3Assert(ptIndex >= 0);
+		if (ptIndex < 0)
+		{
+			ptIndex = 0;
+		}
+		dotOut = maxDot;
+		return ptIndex;
+	}
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+	return b3_maxdot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut);
+#endif
+}
+
+B3_FORCE_INLINE long b3Vector3::minDot(const b3Vector3* array, long array_count, b3Scalar& dotOut) const
+{
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+#if defined B3_USE_SSE
+	const long scalar_cutoff = 10;
+	long b3_mindot_large(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#elif defined B3_USE_NEON
+	const long scalar_cutoff = 4;
+	extern long (*b3_mindot_large)(const float* array, const float* vec, unsigned long array_count, float* dotOut);
+#else
+#error unhandled arch!
+#endif
+
+	if (array_count < scalar_cutoff)
+#endif  //B3_USE_SSE || B3_USE_NEON
+	{
+		b3Scalar minDot = B3_INFINITY;
+		int i = 0;
+		int ptIndex = -1;
+
+		for (i = 0; i < array_count; i++)
+		{
+			b3Scalar dot = array[i].dot(*this);
+
+			if (dot < minDot)
+			{
+				minDot = dot;
+				ptIndex = i;
+			}
+		}
+
+		dotOut = minDot;
+
+		return ptIndex;
+	}
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+	return b3_mindot_large((float*)array, (float*)&m_floats[0], array_count, &dotOut);
+#endif
+}
+
+class b3Vector4 : public b3Vector3
+{
+public:
+	B3_FORCE_INLINE b3Vector4 absolute4() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask));
+#elif defined(B3_USE_NEON)
+		return b3Vector4(vabsq_f32(mVec128));
+#else
+		return b3MakeVector4(
+			b3Fabs(m_floats[0]),
+			b3Fabs(m_floats[1]),
+			b3Fabs(m_floats[2]),
+			b3Fabs(m_floats[3]));
+#endif
+	}
+
+	b3Scalar getW() const { return m_floats[3]; }
+
+	B3_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		b3Scalar maxVal = b3Scalar(-B3_LARGE_FLOAT);
+		if (m_floats[0] > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_floats[0];
+		}
+		if (m_floats[1] > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_floats[1];
+		}
+		if (m_floats[2] > maxVal)
+		{
+			maxIndex = 2;
+			maxVal = m_floats[2];
+		}
+		if (m_floats[3] > maxVal)
+		{
+			maxIndex = 3;
+		}
+
+		return maxIndex;
+	}
+
+	B3_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		b3Scalar minVal = b3Scalar(B3_LARGE_FLOAT);
+		if (m_floats[0] < minVal)
+		{
+			minIndex = 0;
+			minVal = m_floats[0];
+		}
+		if (m_floats[1] < minVal)
+		{
+			minIndex = 1;
+			minVal = m_floats[1];
+		}
+		if (m_floats[2] < minVal)
+		{
+			minIndex = 2;
+			minVal = m_floats[2];
+		}
+		if (m_floats[3] < minVal)
+		{
+			minIndex = 3;
+			minVal = m_floats[3];
+		}
+
+		return minIndex;
+	}
+
+	B3_FORCE_INLINE int closestAxis4() const
+	{
+		return absolute4().maxAxis4();
+	}
+
+	/**@brief Set x,y,z and zero w
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+
+	/*		void getValue(b3Scalar *m) const
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+	/**@brief Set the values
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
+};
+
+///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void b3SwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
+{
+#ifdef B3_USE_DOUBLE_PRECISION
+	unsigned char* dest = (unsigned char*)&destVal;
+	unsigned char* src = (unsigned char*)&sourceVal;
+	dest[0] = src[7];
+	dest[1] = src[6];
+	dest[2] = src[5];
+	dest[3] = src[4];
+	dest[4] = src[3];
+	dest[5] = src[2];
+	dest[6] = src[1];
+	dest[7] = src[0];
+#else
+	unsigned char* dest = (unsigned char*)&destVal;
+	unsigned char* src = (unsigned char*)&sourceVal;
+	dest[0] = src[3];
+	dest[1] = src[2];
+	dest[2] = src[1];
+	dest[3] = src[0];
+#endif  //B3_USE_DOUBLE_PRECISION
+}
+///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void b3SwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec)
+{
+	for (int i = 0; i < 4; i++)
+	{
+		b3SwapScalarEndian(sourceVec[i], destVec[i]);
+	}
+}
+
+///b3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+B3_FORCE_INLINE void b3UnSwapVector3Endian(b3Vector3& vector)
+{
+	b3Vector3 swappedVec;
+	for (int i = 0; i < 4; i++)
+	{
+		b3SwapScalarEndian(vector[i], swappedVec[i]);
+	}
+	vector = swappedVec;
+}
+
+template <class T>
+B3_FORCE_INLINE void b3PlaneSpace1(const T& n, T& p, T& q)
+{
+	if (b3Fabs(n[2]) > B3_SQRT12)
+	{
+		// choose p in y-z plane
+		b3Scalar a = n[1] * n[1] + n[2] * n[2];
+		b3Scalar k = b3RecipSqrt(a);
+		p[0] = 0;
+		p[1] = -n[2] * k;
+		p[2] = n[1] * k;
+		// set q = n x p
+		q[0] = a * k;
+		q[1] = -n[0] * p[2];
+		q[2] = n[0] * p[1];
+	}
+	else
+	{
+		// choose p in x-y plane
+		b3Scalar a = n[0] * n[0] + n[1] * n[1];
+		b3Scalar k = b3RecipSqrt(a);
+		p[0] = -n[1] * k;
+		p[1] = n[0] * k;
+		p[2] = 0;
+		// set q = n x p
+		q[0] = -n[2] * p[1];
+		q[1] = n[2] * p[0];
+		q[2] = a * k;
+	}
+}
+
+struct b3Vector3FloatData
+{
+	float m_floats[4];
+};
+
+struct b3Vector3DoubleData
+{
+	double m_floats[4];
+};
+
+B3_FORCE_INLINE void b3Vector3::serializeFloat(struct b3Vector3FloatData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i = 0; i < 4; i++)
+		dataOut.m_floats[i] = float(m_floats[i]);
+}
+
+B3_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct b3Vector3FloatData& dataIn)
+{
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = b3Scalar(dataIn.m_floats[i]);
+}
+
+B3_FORCE_INLINE void b3Vector3::serializeDouble(struct b3Vector3DoubleData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i = 0; i < 4; i++)
+		dataOut.m_floats[i] = double(m_floats[i]);
+}
+
+B3_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct b3Vector3DoubleData& dataIn)
+{
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = b3Scalar(dataIn.m_floats[i]);
+}
+
+B3_FORCE_INLINE void b3Vector3::serialize(struct b3Vector3Data& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i = 0; i < 4; i++)
+		dataOut.m_floats[i] = m_floats[i];
+}
+
+B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
+{
+	for (int i = 0; i < 4; i++)
+		m_floats[i] = dataIn.m_floats[i];
+}
+
+inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z)
+{
+	b3Vector3 tmp;
+	tmp.setValue(x, y, z);
+	return tmp;
+}
+
+inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w)
+{
+	b3Vector3 tmp;
+	tmp.setValue(x, y, z);
+	tmp.w = w;
+	return tmp;
+}
+
+inline b3Vector4 b3MakeVector4(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w)
+{
+	b3Vector4 tmp;
+	tmp.setValue(x, y, z, w);
+	return tmp;
+}
+
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+
+inline b3Vector3 b3MakeVector3(b3SimdFloat4 v)
+{
+	b3Vector3 tmp;
+	tmp.set128(v);
+	return tmp;
+}
+
+inline b3Vector4 b3MakeVector4(b3SimdFloat4 vec)
+{
+	b3Vector4 tmp;
+	tmp.set128(vec);
+	return tmp;
+}
+
+#endif
+
+#endif  //B3_VECTOR3_H

Engine/lib/bullet/src/Bullet3Common/premake4.lua

@@ -0,0 +1,16 @@
+	project "Bullet3Common"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	if os.is("Linux") then
+	    buildoptions{"-fPIC"}
+	end
+
+	includedirs {".."}
+
+	files {
+		"*.cpp",
+		"*.h"
+	}

Engine/lib/bullet/src/Bullet3Common/shared/b3Float4.h

@@ -0,0 +1,90 @@
+#ifndef B3_FLOAT4_H
+#define B3_FLOAT4_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+
+#ifdef __cplusplus
+#include "Bullet3Common/b3Vector3.h"
+#define b3Float4 b3Vector3
+#define b3Float4ConstArg const b3Vector3&
+#define b3Dot3F4 b3Dot
+#define b3Cross3 b3Cross
+#define b3MakeFloat4 b3MakeVector3
+inline b3Vector3 b3Normalized(const b3Vector3& vec)
+{
+	return vec.normalized();
+}
+
+inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
+{
+	return v.normalized();
+}
+
+inline b3Float4 b3MaxFloat4(const b3Float4& a, const b3Float4& b)
+{
+	b3Float4 tmp = a;
+	tmp.setMax(b);
+	return tmp;
+}
+inline b3Float4 b3MinFloat4(const b3Float4& a, const b3Float4& b)
+{
+	b3Float4 tmp = a;
+	tmp.setMin(b);
+	return tmp;
+}
+
+#else
+typedef float4 b3Float4;
+#define b3Float4ConstArg const b3Float4
+#define b3MakeFloat4 (float4)
+float b3Dot3F4(b3Float4ConstArg v0, b3Float4ConstArg v1)
+{
+	float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
+	float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
+	return dot(a1, b1);
+}
+b3Float4 b3Cross3(b3Float4ConstArg v0, b3Float4ConstArg v1)
+{
+	float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
+	float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
+	return cross(a1, b1);
+}
+#define b3MinFloat4 min
+#define b3MaxFloat4 max
+
+#define b3Normalized(a) normalize(a)
+
+#endif
+
+inline bool b3IsAlmostZero(b3Float4ConstArg v)
+{
+	if (b3Fabs(v.x) > 1e-6 || b3Fabs(v.y) > 1e-6 || b3Fabs(v.z) > 1e-6)
+		return false;
+	return true;
+}
+
+inline int b3MaxDot(b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut)
+{
+	float maxDot = -B3_INFINITY;
+	int i = 0;
+	int ptIndex = -1;
+	for (i = 0; i < vecLen; i++)
+	{
+		float dot = b3Dot3F4(vecArray[i], vec);
+
+		if (dot > maxDot)
+		{
+			maxDot = dot;
+			ptIndex = i;
+		}
+	}
+	b3Assert(ptIndex >= 0);
+	if (ptIndex < 0)
+	{
+		ptIndex = 0;
+	}
+	*dotOut = maxDot;
+	return ptIndex;
+}
+
+#endif  //B3_FLOAT4_H

Engine/lib/bullet/src/Bullet3Common/shared/b3Int2.h

@@ -0,0 +1,63 @@
+/*
+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 B3_INT2_H
+#define B3_INT2_H
+
+#ifdef __cplusplus
+
+struct b3UnsignedInt2
+{
+	union {
+		struct
+		{
+			unsigned int x, y;
+		};
+		struct
+		{
+			unsigned int s[2];
+		};
+	};
+};
+
+struct b3Int2
+{
+	union {
+		struct
+		{
+			int x, y;
+		};
+		struct
+		{
+			int s[2];
+		};
+	};
+};
+
+inline b3Int2 b3MakeInt2(int x, int y)
+{
+	b3Int2 v;
+	v.s[0] = x;
+	v.s[1] = y;
+	return v;
+}
+#else
+
+#define b3UnsignedInt2 uint2
+#define b3Int2 int2
+#define b3MakeInt2 (int2)
+
+#endif  //__cplusplus
+#endif

Engine/lib/bullet/src/Bullet3Common/shared/b3Int4.h

@@ -0,0 +1,71 @@
+#ifndef B3_INT4_H
+#define B3_INT4_H
+
+#ifdef __cplusplus
+
+#include "Bullet3Common/b3Scalar.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3UnsignedInt4
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	union {
+		struct
+		{
+			unsigned int x, y, z, w;
+		};
+		struct
+		{
+			unsigned int s[4];
+		};
+	};
+};
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3Int4
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	union {
+		struct
+		{
+			int x, y, z, w;
+		};
+		struct
+		{
+			int s[4];
+		};
+	};
+};
+
+B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0)
+{
+	b3Int4 v;
+	v.s[0] = x;
+	v.s[1] = y;
+	v.s[2] = z;
+	v.s[3] = w;
+	return v;
+}
+
+B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0)
+{
+	b3UnsignedInt4 v;
+	v.s[0] = x;
+	v.s[1] = y;
+	v.s[2] = z;
+	v.s[3] = w;
+	return v;
+}
+
+#else
+
+#define b3UnsignedInt4 uint4
+#define b3Int4 int4
+#define b3MakeInt4 (int4)
+#define b3MakeUnsignedInt4 (uint4)
+
+#endif  //__cplusplus
+
+#endif  //B3_INT4_H

Engine/lib/bullet/src/Bullet3Common/shared/b3Mat3x3.h

@@ -0,0 +1,157 @@
+
+#ifndef B3_MAT3x3_H
+#define B3_MAT3x3_H
+
+#include "Bullet3Common/shared/b3Quat.h"
+
+#ifdef __cplusplus
+
+#include "Bullet3Common/b3Matrix3x3.h"
+
+#define b3Mat3x3 b3Matrix3x3
+#define b3Mat3x3ConstArg const b3Matrix3x3&
+
+inline b3Mat3x3 b3QuatGetRotationMatrix(b3QuatConstArg quat)
+{
+	return b3Mat3x3(quat);
+}
+
+inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
+{
+	return mat.absolute();
+}
+
+#define b3GetRow(m, row) m.getRow(row)
+
+__inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
+{
+	return b * a;
+}
+
+#else
+
+typedef struct
+{
+	b3Float4 m_row[3];
+} b3Mat3x3;
+
+#define b3Mat3x3ConstArg const b3Mat3x3
+#define b3GetRow(m, row) (m.m_row[row])
+
+inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)
+{
+	b3Float4 quat2 = (b3Float4)(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, 0.f);
+	b3Mat3x3 out;
+
+	out.m_row[0].x = 1 - 2 * quat2.y - 2 * quat2.z;
+	out.m_row[0].y = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
+	out.m_row[0].z = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
+	out.m_row[0].w = 0.f;
+
+	out.m_row[1].x = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
+	out.m_row[1].y = 1 - 2 * quat2.x - 2 * quat2.z;
+	out.m_row[1].z = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
+	out.m_row[1].w = 0.f;
+
+	out.m_row[2].x = 2 * quat.x * quat.z - 2 * quat.w * quat.y;
+	out.m_row[2].y = 2 * quat.y * quat.z + 2 * quat.w * quat.x;
+	out.m_row[2].z = 1 - 2 * quat2.x - 2 * quat2.y;
+	out.m_row[2].w = 0.f;
+
+	return out;
+}
+
+inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
+{
+	b3Mat3x3 out;
+	out.m_row[0] = fabs(matIn.m_row[0]);
+	out.m_row[1] = fabs(matIn.m_row[1]);
+	out.m_row[2] = fabs(matIn.m_row[2]);
+	return out;
+}
+
+__inline b3Mat3x3 mtZero();
+
+__inline b3Mat3x3 mtIdentity();
+
+__inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
+
+__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
+
+__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
+
+__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
+
+__inline b3Mat3x3 mtZero()
+{
+	b3Mat3x3 m;
+	m.m_row[0] = (b3Float4)(0.f);
+	m.m_row[1] = (b3Float4)(0.f);
+	m.m_row[2] = (b3Float4)(0.f);
+	return m;
+}
+
+__inline b3Mat3x3 mtIdentity()
+{
+	b3Mat3x3 m;
+	m.m_row[0] = (b3Float4)(1, 0, 0, 0);
+	m.m_row[1] = (b3Float4)(0, 1, 0, 0);
+	m.m_row[2] = (b3Float4)(0, 0, 1, 0);
+	return m;
+}
+
+__inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
+{
+	b3Mat3x3 out;
+	out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
+	out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
+	out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
+	return out;
+}
+
+__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
+{
+	b3Mat3x3 transB;
+	transB = mtTranspose(b);
+	b3Mat3x3 ans;
+	//	why this doesn't run when 0ing in the for{}
+	a.m_row[0].w = 0.f;
+	a.m_row[1].w = 0.f;
+	a.m_row[2].w = 0.f;
+	for (int i = 0; i < 3; i++)
+	{
+		//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = b3Dot3F4(a.m_row[i], transB.m_row[0]);
+		ans.m_row[i].y = b3Dot3F4(a.m_row[i], transB.m_row[1]);
+		ans.m_row[i].z = b3Dot3F4(a.m_row[i], transB.m_row[2]);
+		ans.m_row[i].w = 0.f;
+	}
+	return ans;
+}
+
+__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
+{
+	b3Float4 ans;
+	ans.x = b3Dot3F4(a.m_row[0], b);
+	ans.y = b3Dot3F4(a.m_row[1], b);
+	ans.z = b3Dot3F4(a.m_row[2], b);
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
+{
+	b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	b3Float4 ans;
+	ans.x = b3Dot3F4(a, colx);
+	ans.y = b3Dot3F4(a, coly);
+	ans.z = b3Dot3F4(a, colz);
+	return ans;
+}
+
+#endif
+
+#endif  //B3_MAT3x3_H

Engine/lib/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h

@@ -0,0 +1,41 @@
+#ifndef B3_PLATFORM_DEFINITIONS_H
+#define B3_PLATFORM_DEFINITIONS_H
+
+struct MyTest
+{
+	int bla;
+};
+
+#ifdef __cplusplus
+//#define b3ConstArray(a) const b3AlignedObjectArray<a>&
+#define b3ConstArray(a) const a *
+#define b3AtomicInc(a) ((*a)++)
+
+inline int b3AtomicAdd(volatile int *p, int val)
+{
+	int oldValue = *p;
+	int newValue = oldValue + val;
+	*p = newValue;
+	return oldValue;
+}
+
+#define __global
+
+#define B3_STATIC static
+#else
+//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
+#define B3_LARGE_FLOAT 1e18f
+#define B3_INFINITY 1e18f
+#define b3Assert(a)
+#define b3ConstArray(a) __global const a *
+#define b3AtomicInc atomic_inc
+#define b3AtomicAdd atomic_add
+#define b3Fabs fabs
+#define b3Sqrt native_sqrt
+#define b3Sin native_sin
+#define b3Cos native_cos
+
+#define B3_STATIC
+#endif
+
+#endif

Engine/lib/bullet/src/Bullet3Common/shared/b3Quat.h

@@ -0,0 +1,100 @@
+#ifndef B3_QUAT_H
+#define B3_QUAT_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+#include "Bullet3Common/shared/b3Float4.h"
+
+#ifdef __cplusplus
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+
+#define b3Quat b3Quaternion
+#define b3QuatConstArg const b3Quaternion&
+inline b3Quat b3QuatInverse(b3QuatConstArg orn)
+{
+	return orn.inverse();
+}
+
+inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
+{
+	b3Transform tr;
+	tr.setOrigin(translation);
+	tr.setRotation(orientation);
+	return tr(point);
+}
+
+#else
+typedef float4 b3Quat;
+#define b3QuatConstArg const b3Quat
+
+inline float4 b3FastNormalize4(float4 v)
+{
+	v = (float4)(v.xyz, 0.f);
+	return fast_normalize(v);
+}
+
+inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
+inline b3Quat b3QuatNormalized(b3QuatConstArg in);
+inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
+inline b3Quat b3QuatInvert(b3QuatConstArg q);
+inline b3Quat b3QuatInverse(b3QuatConstArg q);
+
+inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
+{
+	b3Quat ans;
+	ans = b3Cross3(a, b);
+	ans += a.w * b + b.w * a;
+	//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
+	ans.w = a.w * b.w - b3Dot3F4(a, b);
+	return ans;
+}
+
+inline b3Quat b3QuatNormalized(b3QuatConstArg in)
+{
+	b3Quat q;
+	q = in;
+	//return b3FastNormalize4(in);
+	float len = native_sqrt(dot(q, q));
+	if (len > 0.f)
+	{
+		q *= 1.f / len;
+	}
+	else
+	{
+		q.x = q.y = q.z = 0.f;
+		q.w = 1.f;
+	}
+	return q;
+}
+inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
+{
+	b3Quat qInv = b3QuatInvert(q);
+	float4 vcpy = vec;
+	vcpy.w = 0.f;
+	float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
+	return out;
+}
+
+inline b3Quat b3QuatInverse(b3QuatConstArg q)
+{
+	return (b3Quat)(-q.xyz, q.w);
+}
+
+inline b3Quat b3QuatInvert(b3QuatConstArg q)
+{
+	return (b3Quat)(-q.xyz, q.w);
+}
+
+inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
+{
+	return b3QuatRotate(b3QuatInvert(q), vec);
+}
+
+inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
+{
+	return b3QuatRotate(orientation, point) + (translation);
+}
+
+#endif
+
+#endif  //B3_QUAT_H

Engine/lib/bullet/src/Bullet3Dynamics/CMakeLists.txt

@@ -0,0 +1,61 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Dynamics_SRCS
+	b3CpuRigidBodyPipeline.cpp
+	ConstraintSolver/b3FixedConstraint.cpp
+	ConstraintSolver/b3Generic6DofConstraint.cpp
+	ConstraintSolver/b3PgsJacobiSolver.cpp
+	ConstraintSolver/b3Point2PointConstraint.cpp
+	ConstraintSolver/b3TypedConstraint.cpp
+)
+
+SET(Bullet3Dynamics_HDRS
+	  b3CpuRigidBodyPipeline.h
+	ConstraintSolver/b3ContactSolverInfo.h
+	ConstraintSolver/b3FixedConstraint.h
+	ConstraintSolver/b3Generic6DofConstraint.h
+	ConstraintSolver/b3JacobianEntry.h
+	ConstraintSolver/b3PgsJacobiSolver.h
+	ConstraintSolver/b3Point2PointConstraint.h
+	ConstraintSolver/b3SolverBody.h
+	ConstraintSolver/b3SolverConstraint.h
+	ConstraintSolver/b3TypedConstraint.h
+	shared/b3ContactConstraint4.h
+	shared/b3ConvertConstraint4.h
+	shared/b3Inertia.h
+	shared/b3IntegrateTransforms.h
+)
+
+ADD_LIBRARY(Bullet3Dynamics ${Bullet3Dynamics_SRCS} ${Bullet3Dynamics_HDRS})
+if (BUILD_SHARED_LIBS)
+  target_link_libraries(Bullet3Dynamics Bullet3Collision)
+endif ()
+SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Dynamics DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Dynamics
+					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)
+			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(Bullet3Dynamics PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES PUBLIC_HEADER "${Bullet3Dynamics_HDRS}")
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h

@@ -0,0 +1,149 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT_SOLVER_INFO
+#define B3_CONTACT_SOLVER_INFO
+
+#include "Bullet3Common/b3Scalar.h"
+
+enum b3SolverMode
+{
+	B3_SOLVER_RANDMIZE_ORDER = 1,
+	B3_SOLVER_FRICTION_SEPARATE = 2,
+	B3_SOLVER_USE_WARMSTARTING = 4,
+	B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+	B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+	B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+	B3_SOLVER_CACHE_FRIENDLY = 128,
+	B3_SOLVER_SIMD = 256,
+	B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
+	B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+};
+
+struct b3ContactSolverInfoData
+{
+	b3Scalar m_tau;
+	b3Scalar m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	b3Scalar m_friction;
+	b3Scalar m_timeStep;
+	b3Scalar m_restitution;
+	int m_numIterations;
+	b3Scalar m_maxErrorReduction;
+	b3Scalar m_sor;
+	b3Scalar m_erp;        //used as Baumgarte factor
+	b3Scalar m_erp2;       //used in Split Impulse
+	b3Scalar m_globalCfm;  //constraint force mixing
+	int m_splitImpulse;
+	b3Scalar m_splitImpulsePenetrationThreshold;
+	b3Scalar m_splitImpulseTurnErp;
+	b3Scalar m_linearSlop;
+	b3Scalar m_warmstartingFactor;
+
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	b3Scalar m_maxGyroscopicForce;
+	b3Scalar m_singleAxisRollingFrictionThreshold;
+};
+
+struct b3ContactSolverInfo : public b3ContactSolverInfoData
+{
+	inline b3ContactSolverInfo()
+	{
+		m_tau = b3Scalar(0.6);
+		m_damping = b3Scalar(1.0);
+		m_friction = b3Scalar(0.3);
+		m_timeStep = b3Scalar(1.f / 60.f);
+		m_restitution = b3Scalar(0.);
+		m_maxErrorReduction = b3Scalar(20.);
+		m_numIterations = 10;
+		m_erp = b3Scalar(0.2);
+		m_erp2 = b3Scalar(0.8);
+		m_globalCfm = b3Scalar(0.);
+		m_sor = b3Scalar(1.);
+		m_splitImpulse = true;
+		m_splitImpulsePenetrationThreshold = -.04f;
+		m_splitImpulseTurnErp = 0.1f;
+		m_linearSlop = b3Scalar(0.0);
+		m_warmstartingFactor = b3Scalar(0.85);
+		//m_solverMode =  B3_SOLVER_USE_WARMSTARTING |  B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
+		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;  // | B3_SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;                    //unused as of 2.81
+		m_minimumSolverBatchSize = 128;                              //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f;                                ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
+		m_singleAxisRollingFrictionThreshold = 1e30f;                ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+	}
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct b3ContactSolverInfoDoubleData
+{
+	double m_tau;
+	double m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double m_friction;
+	double m_timeStep;
+	double m_restitution;
+	double m_maxErrorReduction;
+	double m_sor;
+	double m_erp;        //used as Baumgarte factor
+	double m_erp2;       //used in Split Impulse
+	double m_globalCfm;  //constraint force mixing
+	double m_splitImpulsePenetrationThreshold;
+	double m_splitImpulseTurnErp;
+	double m_linearSlop;
+	double m_warmstartingFactor;
+	double m_maxGyroscopicForce;
+	double m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	char m_padding[4];
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct b3ContactSolverInfoFloatData
+{
+	float m_tau;
+	float m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float m_friction;
+	float m_timeStep;
+
+	float m_restitution;
+	float m_maxErrorReduction;
+	float m_sor;
+	float m_erp;  //used as Baumgarte factor
+
+	float m_erp2;       //used in Split Impulse
+	float m_globalCfm;  //constraint force mixing
+	float m_splitImpulsePenetrationThreshold;
+	float m_splitImpulseTurnErp;
+
+	float m_linearSlop;
+	float m_warmstartingFactor;
+	float m_maxGyroscopicForce;
+	float m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+
+	int m_splitImpulse;
+	char m_padding[4];
+};
+
+#endif  //B3_CONTACT_SOLVER_INFO

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp

@@ -0,0 +1,103 @@
+
+#include "b3FixedConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Common/b3TransformUtil.h"
+#include <new>
+
+b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
+	: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
+{
+	m_pivotInA = frameInA.getOrigin();
+	m_pivotInB = frameInB.getOrigin();
+	m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
+}
+
+b3FixedConstraint::~b3FixedConstraint()
+{
+}
+
+void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
+{
+	info->m_numConstraintRows = 6;
+	info->nub = 6;
+}
+
+void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+{
+	//fix the 3 linear degrees of freedom
+
+	const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
+	const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
+	const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
+	const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
+
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
+	{
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
+		b3Vector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
+	}
+
+	if (info->m_J2linearAxis)
+	{
+		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
+	}
+
+	b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
+
+	{
+		//	b3Vector3 a2n = -a2;
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
+	}
+
+	// set right hand side for the linear dofs
+	b3Scalar k = info->fps * info->erp;
+	b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = linearError[j];
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+	}
+
+	//fix the 3 angular degrees of freedom
+
+	int start_row = 3;
+	int s = info->rowskip;
+	int start_index = start_row * s;
+
+	// 3 rows to make body rotations equal
+	info->m_J1angularAxis[start_index] = 1;
+	info->m_J1angularAxis[start_index + s + 1] = 1;
+	info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
+	if (info->m_J2angularAxis)
+	{
+		info->m_J2angularAxis[start_index] = -1;
+		info->m_J2angularAxis[start_index + s + 1] = -1;
+		info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
+	}
+
+	// set right hand side for the angular dofs
+
+	b3Vector3 diff;
+	b3Scalar angle;
+	b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
+
+	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
+	diff *= -angle;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
+	}
+}

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h

@@ -0,0 +1,34 @@
+
+#ifndef B3_FIXED_CONSTRAINT_H
+#define B3_FIXED_CONSTRAINT_H
+
+#include "b3TypedConstraint.h"
+
+B3_ATTRIBUTE_ALIGNED16(class)
+b3FixedConstraint : public b3TypedConstraint
+{
+	b3Vector3 m_pivotInA;
+	b3Vector3 m_pivotInB;
+	b3Quaternion m_relTargetAB;
+
+public:
+	b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
+
+	virtual ~b3FixedConstraint();
+
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
+
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
+
+	virtual void setParam(int num, b3Scalar value, int axis = -1)
+	{
+		b3Assert(0);
+	}
+	virtual b3Scalar getParam(int num, int axis = -1) const
+	{
+		b3Assert(0);
+		return 0.f;
+	}
+};
+
+#endif  //B3_FIXED_CONSTRAINT_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp

@@ -0,0 +1,737 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+2007-09-09
+Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+#include "b3Generic6DofConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+#include "Bullet3Common/b3TransformUtil.h"
+#include "Bullet3Common/b3TransformUtil.h"
+#include <new>
+
+#define D6_USE_OBSOLETE_METHOD false
+#define D6_USE_FRAME_OFFSET true
+
+b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
+	: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
+{
+	calculateTransforms(bodies);
+}
+
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
+b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
+{
+	int i = index % 3;
+	int j = index / 3;
+	return mat[i][j];
+}
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
+{
+	//	// rot =  cy*cz          -cy*sz           sy
+	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+	//
+
+	b3Scalar fi = btGetMatrixElem(mat, 2);
+	if (fi < b3Scalar(1.0f))
+	{
+		if (fi > b3Scalar(-1.0f))
+		{
+			xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
+			xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
+			return true;
+		}
+		else
+		{
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
+			xyz[1] = -B3_HALF_PI;
+			xyz[2] = b3Scalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
+		xyz[1] = B3_HALF_PI;
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
+
+int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
+{
+	if (m_loLimit > m_hiLimit)
+	{
+		m_currentLimit = 0;  //Free from violation
+		return 0;
+	}
+	if (test_value < m_loLimit)
+	{
+		m_currentLimit = 1;  //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
+		return 1;
+	}
+	else if (test_value > m_hiLimit)
+	{
+		m_currentLimit = 2;  //High limit violation
+		m_currentLimitError = test_value - m_hiLimit;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
+		return 2;
+	};
+
+	m_currentLimit = 0;  //Free from violation
+	return 0;
+}
+
+//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
+
+//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
+
+int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
+{
+	b3Scalar loLimit = m_lowerLimit[limitIndex];
+	b3Scalar hiLimit = m_upperLimit[limitIndex];
+	if (loLimit > hiLimit)
+	{
+		m_currentLimit[limitIndex] = 0;  //Free from violation
+		m_currentLimitError[limitIndex] = b3Scalar(0.f);
+		return 0;
+	}
+
+	if (test_value < loLimit)
+	{
+		m_currentLimit[limitIndex] = 2;  //low limit violation
+		m_currentLimitError[limitIndex] = test_value - loLimit;
+		return 2;
+	}
+	else if (test_value > hiLimit)
+	{
+		m_currentLimit[limitIndex] = 1;  //High limit violation
+		m_currentLimitError[limitIndex] = test_value - hiLimit;
+		return 1;
+	};
+
+	m_currentLimit[limitIndex] = 0;  //Free from violation
+	m_currentLimitError[limitIndex] = b3Scalar(0.f);
+	return 0;
+}
+
+//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
+
+void b3Generic6DofConstraint::calculateAngleInfo()
+{
+	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
+	// in euler angle mode we do not actually constrain the angular velocity
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
+	b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+	b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+	m_calculatedAxis[1] = axis2.cross(axis0);
+	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+	m_calculatedAxis[0].normalize();
+	m_calculatedAxis[1].normalize();
+	m_calculatedAxis[2].normalize();
+}
+
+static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
+{
+	b3Transform tr(body.m_quat, body.m_pos);
+	return tr;
+}
+
+void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
+{
+	b3Transform transA;
+	b3Transform transB;
+	transA = getCenterOfMassTransform(bodies[m_rbA]);
+	transB = getCenterOfMassTransform(bodies[m_rbB]);
+	calculateTransforms(transA, transB, bodies);
+}
+
+void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
+{
+	m_calculatedTransformA = transA * m_frameInA;
+	m_calculatedTransformB = transB * m_frameInB;
+	calculateLinearInfo();
+	calculateAngleInfo();
+	if (m_useOffsetForConstraintFrame)
+	{  //  get weight factors depending on masses
+		b3Scalar miA = bodies[m_rbA].m_invMass;
+		b3Scalar miB = bodies[m_rbB].m_invMass;
+		m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
+		b3Scalar miS = miA + miB;
+		if (miS > b3Scalar(0.f))
+		{
+			m_factA = miB / miS;
+		}
+		else
+		{
+			m_factA = b3Scalar(0.5f);
+		}
+		m_factB = b3Scalar(1.0f) - m_factA;
+	}
+}
+
+bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
+{
+	b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+	m_angularLimits[axis_index].m_currentPosition = angle;
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
+}
+
+void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
+{
+	//prepare constraint
+	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
+	info->m_numConstraintRows = 0;
+	info->nub = 6;
+	int i;
+	//test linear limits
+	for (i = 0; i < 3; i++)
+	{
+		if (m_linearLimits.needApplyForce(i))
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+	//test angular limits
+	for (i = 0; i < 3; i++)
+	{
+		if (testAngularLimitMotor(i))
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+	//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
+}
+
+void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
+{
+	//pre-allocate all 6
+	info->m_numConstraintRows = 6;
+	info->nub = 0;
+}
+
+void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+{
+	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
+	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
+	const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
+	const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
+	const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
+	const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
+
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
+	}
+	else
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
+	}
+}
+
+void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
+{
+	//prepare constraint
+	calculateTransforms(transA, transB, bodies);
+
+	int i;
+	for (i = 0; i < 3; i++)
+	{
+		testAngularLimitMotor(i);
+	}
+
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
+	}
+	else
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
+	}
+}
+
+int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
+{
+	//	int row = 0;
+	//solve linear limits
+	b3RotationalLimitMotor limot;
+	for (int i = 0; i < 3; i++)
+	{
+		if (m_linearLimits.needApplyForce(i))
+		{  // re-use rotational motor code
+			limot.m_bounce = b3Scalar(0.f);
+			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping = m_linearLimits.m_damping;
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce = b3Scalar(0.f);
+			limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
+			b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+			int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
+			limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if (m_useOffsetForConstraintFrame)
+			{
+				int indx1 = (i + 1) % 3;
+				int indx2 = (i + 2) % 3;
+				int rotAllowed = 1;  // rotations around orthos to current axis
+				if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				{
+					rotAllowed = 0;
+				}
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
+			}
+			else
+			{
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
+			}
+		}
+	}
+	return row;
+}
+
+int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
+{
+	b3Generic6DofConstraint* d6constraint = this;
+	int row = row_offset;
+	//solve angular limits
+	for (int i = 0; i < 3; i++)
+	{
+		if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		{
+			b3Vector3 axis = d6constraint->getAxis(i);
+			int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
+			if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
+			{
+				m_angularLimits[i].m_normalCFM = info->cfm[0];
+			}
+			if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
+			{
+				m_angularLimits[i].m_stopCFM = info->cfm[0];
+			}
+			if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
+			{
+				m_angularLimits[i].m_stopERP = info->erp;
+			}
+			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+										 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
+		}
+	}
+
+	return row;
+}
+
+void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
+{
+	(void)timeStep;
+}
+
+void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
+{
+	m_frameInA = frameA;
+	m_frameInB = frameB;
+
+	calculateTransforms(bodies);
+}
+
+b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
+{
+	return m_calculatedAxis[axis_index];
+}
+
+b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+	return m_calculatedLinearDiff[axisIndex];
+}
+
+b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
+{
+	return m_calculatedAxisAngleDiff[axisIndex];
+}
+
+void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
+{
+	b3Scalar imA = bodies[m_rbA].m_invMass;
+	b3Scalar imB = bodies[m_rbB].m_invMass;
+	b3Scalar weight;
+	if (imB == b3Scalar(0.0))
+	{
+		weight = b3Scalar(1.0);
+	}
+	else
+	{
+		weight = imA / (imA + imB);
+	}
+	const b3Vector3& pA = m_calculatedTransformA.getOrigin();
+	const b3Vector3& pB = m_calculatedTransformB.getOrigin();
+	m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
+	return;
+}
+
+void b3Generic6DofConstraint::calculateLinearInfo()
+{
+	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+	for (int i = 0; i < 3; i++)
+	{
+		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
+		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+	}
+}
+
+int b3Generic6DofConstraint::get_limit_motor_info2(
+	b3RotationalLimitMotor* limot,
+	const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+	b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
+{
+	int srow = row * info->rowskip;
+	bool powered = limot->m_enableMotor;
+	int limit = limot->m_currentLimit;
+	if (powered || limit)
+	{  // if the joint is powered, or has joint limits, add in the extra row
+		b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+		b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+		if (J1)
+		{
+			J1[srow + 0] = ax1[0];
+			J1[srow + 1] = ax1[1];
+			J1[srow + 2] = ax1[2];
+		}
+		if (J2)
+		{
+			J2[srow + 0] = -ax1[0];
+			J2[srow + 1] = -ax1[1];
+			J2[srow + 2] = -ax1[2];
+		}
+		if ((!rotational))
+		{
+			if (m_useOffsetForConstraintFrame)
+			{
+				b3Vector3 tmpA, tmpB, relA, relB;
+				// get vector from bodyB to frameB in WCS
+				relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+				// get its projection to constraint axis
+				b3Vector3 projB = ax1 * relB.dot(ax1);
+				// get vector directed from bodyB to constraint axis (and orthogonal to it)
+				b3Vector3 orthoB = relB - projB;
+				// same for bodyA
+				relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+				b3Vector3 projA = ax1 * relA.dot(ax1);
+				b3Vector3 orthoA = relA - projA;
+				// get desired offset between frames A and B along constraint axis
+				b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+				// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+				b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
+				// get offset vectors relA and relB
+				relA = orthoA + totalDist * m_factA;
+				relB = orthoB - totalDist * m_factB;
+				tmpA = relA.cross(ax1);
+				tmpB = relB.cross(ax1);
+				if (m_hasStaticBody && (!rotAllowed))
+				{
+					tmpA *= m_factA;
+					tmpB *= m_factB;
+				}
+				int i;
+				for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+				for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
+			}
+			else
+			{
+				b3Vector3 ltd;  // Linear Torque Decoupling vector
+				b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+				ltd = c.cross(ax1);
+				info->m_J1angularAxis[srow + 0] = ltd[0];
+				info->m_J1angularAxis[srow + 1] = ltd[1];
+				info->m_J1angularAxis[srow + 2] = ltd[2];
+
+				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+				ltd = -c.cross(ax1);
+				info->m_J2angularAxis[srow + 0] = ltd[0];
+				info->m_J2angularAxis[srow + 1] = ltd[1];
+				info->m_J2angularAxis[srow + 2] = ltd[2];
+			}
+		}
+		// if we're limited low and high simultaneously, the joint motor is
+		// ineffective
+		if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+		info->m_constraintError[srow] = b3Scalar(0.f);
+		if (powered)
+		{
+			info->cfm[srow] = limot->m_normalCFM;
+			if (!limit)
+			{
+				b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+				b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
+												   limot->m_loLimit,
+												   limot->m_hiLimit,
+												   tag_vel,
+												   info->fps * limot->m_stopERP);
+				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
+				info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+			}
+		}
+		if (limit)
+		{
+			b3Scalar k = info->fps * limot->m_stopERP;
+			if (!rotational)
+			{
+				info->m_constraintError[srow] += k * limot->m_currentLimitError;
+			}
+			else
+			{
+				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+			}
+			info->cfm[srow] = limot->m_stopCFM;
+			if (limot->m_loLimit == limot->m_hiLimit)
+			{  // limited low and high simultaneously
+				info->m_lowerLimit[srow] = -B3_INFINITY;
+				info->m_upperLimit[srow] = B3_INFINITY;
+			}
+			else
+			{
+				if (limit == 1)
+				{
+					info->m_lowerLimit[srow] = 0;
+					info->m_upperLimit[srow] = B3_INFINITY;
+				}
+				else
+				{
+					info->m_lowerLimit[srow] = -B3_INFINITY;
+					info->m_upperLimit[srow] = 0;
+				}
+				// deal with bounce
+				if (limot->m_bounce > 0)
+				{
+					// calculate joint velocity
+					b3Scalar vel;
+					if (rotational)
+					{
+						vel = angVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= angVelB.dot(ax1);
+					}
+					else
+					{
+						vel = linVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= linVelB.dot(ax1);
+					}
+					// only apply bounce if the velocity is incoming, and if the
+					// resulting c[] exceeds what we already have.
+					if (limit == 1)
+					{
+						if (vel < 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc > info->m_constraintError[srow])
+								info->m_constraintError[srow] = newc;
+						}
+					}
+					else
+					{
+						if (vel > 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc < info->m_constraintError[srow])
+								info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+		}
+		return 1;
+	}
+	else
+		return 0;
+}
+
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
+{
+	if ((axis >= 0) && (axis < 3))
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_STOP_ERP:
+				m_linearLimits.m_stopERP[axis] = value;
+				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case B3_CONSTRAINT_STOP_CFM:
+				m_linearLimits.m_stopCFM[axis] = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case B3_CONSTRAINT_CFM:
+				m_linearLimits.m_normalCFM[axis] = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+	else if ((axis >= 3) && (axis < 6))
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_STOP_ERP:
+				m_angularLimits[axis - 3].m_stopERP = value;
+				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case B3_CONSTRAINT_STOP_CFM:
+				m_angularLimits[axis - 3].m_stopCFM = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case B3_CONSTRAINT_CFM:
+				m_angularLimits[axis - 3].m_normalCFM = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+	else
+	{
+		b3AssertConstrParams(0);
+	}
+}
+
+///return the local value of parameter
+b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
+{
+	b3Scalar retVal = 0;
+	if ((axis >= 0) && (axis < 3))
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_STOP_ERP:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_stopERP[axis];
+				break;
+			case B3_CONSTRAINT_STOP_CFM:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_stopCFM[axis];
+				break;
+			case B3_CONSTRAINT_CFM:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_normalCFM[axis];
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+	else if ((axis >= 3) && (axis < 6))
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_STOP_ERP:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_stopERP;
+				break;
+			case B3_CONSTRAINT_STOP_CFM:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_stopCFM;
+				break;
+			case B3_CONSTRAINT_CFM:
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_normalCFM;
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+	else
+	{
+		b3AssertConstrParams(0);
+	}
+	return retVal;
+}
+
+void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
+{
+	b3Vector3 zAxis = axis1.normalized();
+	b3Vector3 yAxis = axis2.normalized();
+	b3Vector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
+
+	b3Transform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
+
+	// now get constraint frame in local coordinate systems
+	m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
+	m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
+
+	calculateTransforms(bodies);
+}

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h

@@ -0,0 +1,517 @@
+/*
+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.
+*/
+
+/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+b3Generic6DofConstraint Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
+#define B3_GENERIC_6DOF_CONSTRAINT_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "b3JacobianEntry.h"
+#include "b3TypedConstraint.h"
+
+struct b3RigidBodyData;
+
+//! Rotation Limit structure for generic joints
+class b3RotationalLimitMotor
+{
+public:
+	//! limit_parameters
+	//!@{
+	b3Scalar m_loLimit;         //!< joint limit
+	b3Scalar m_hiLimit;         //!< joint limit
+	b3Scalar m_targetVelocity;  //!< target motor velocity
+	b3Scalar m_maxMotorForce;   //!< max force on motor
+	b3Scalar m_maxLimitForce;   //!< max force on limit
+	b3Scalar m_damping;         //!< Damping.
+	b3Scalar m_limitSoftness;   //! Relaxation factor
+	b3Scalar m_normalCFM;       //!< Constraint force mixing factor
+	b3Scalar m_stopERP;         //!< Error tolerance factor when joint is at limit
+	b3Scalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
+	b3Scalar m_bounce;          //!< restitution factor
+	bool m_enableMotor;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	b3Scalar m_currentLimitError;  //!  How much is violated this limit
+	b3Scalar m_currentPosition;    //!  current value of angle
+	int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
+	b3Scalar m_accumulatedImpulse;
+	//!@}
+
+	b3RotationalLimitMotor()
+	{
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
+		m_normalCFM = 0.f;
+		m_stopERP = 0.2f;
+		m_stopCFM = 0.f;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+	}
+
+	b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
+	{
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
+		m_normalCFM = limot.m_normalCFM;
+		m_stopERP = limot.m_stopERP;
+		m_stopCFM = limot.m_stopCFM;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
+
+	//! Is limited
+	bool isLimited()
+	{
+		if (m_loLimit > m_hiLimit) return false;
+		return true;
+	}
+
+	//! Need apply correction
+	bool needApplyTorques()
+	{
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
+
+	//! calculates  error
+	/*!
+	calculates m_currentLimit and m_currentLimitError.
+	*/
+	int testLimitValue(b3Scalar test_value);
+
+	//! apply the correction impulses for two bodies
+	b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
+};
+
+class b3TranslationalLimitMotor
+{
+public:
+	b3Vector3 m_lowerLimit;  //!< the constraint lower limits
+	b3Vector3 m_upperLimit;  //!< the constraint upper limits
+	b3Vector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	b3Vector3 m_normalCFM;          //!< Constraint force mixing factor
+	b3Vector3 m_stopERP;            //!< Error tolerance factor when joint is at limit
+	b3Vector3 m_stopCFM;            //!< Constraint force mixing factor when joint is at limit
+	b3Vector3 m_targetVelocity;     //!< target motor velocity
+	b3Vector3 m_maxMotorForce;      //!< max force on motor
+	b3Vector3 m_currentLimitError;  //!  How much is violated this limit
+	b3Vector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
+	b3Scalar m_limitSoftness;       //!< Softness for linear limit
+	b3Scalar m_damping;             //!< Damping for linear limit
+	b3Scalar m_restitution;         //! Bounce parameter for linear limit
+	//!@}
+	bool m_enableMotor[3];
+	int m_currentLimit[3];  //!< 0=free, 1=at lower limit, 2=at upper limit
+
+	b3TranslationalLimitMotor()
+	{
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
+		m_normalCFM.setValue(0.f, 0.f, 0.f);
+		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
+		m_stopCFM.setValue(0.f, 0.f, 0.f);
+
+		m_limitSoftness = 0.7f;
+		m_damping = b3Scalar(1.0f);
+		m_restitution = b3Scalar(0.5f);
+		for (int i = 0; i < 3; i++)
+		{
+			m_enableMotor[i] = false;
+			m_targetVelocity[i] = b3Scalar(0.f);
+			m_maxMotorForce[i] = b3Scalar(0.f);
+		}
+	}
+
+	b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
+	{
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
+
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
+		m_normalCFM = other.m_normalCFM;
+		m_stopERP = other.m_stopERP;
+		m_stopCFM = other.m_stopCFM;
+
+		for (int i = 0; i < 3; i++)
+		{
+			m_enableMotor[i] = other.m_enableMotor[i];
+			m_targetVelocity[i] = other.m_targetVelocity[i];
+			m_maxMotorForce[i] = other.m_maxMotorForce[i];
+		}
+	}
+
+	//! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+	inline bool needApplyForce(int limitIndex)
+	{
+		if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+		return true;
+	}
+	int testLimitValue(int limitIndex, b3Scalar test_value);
+
+	b3Scalar solveLinearAxis(
+		b3Scalar timeStep,
+		b3Scalar jacDiagABInv,
+		b3RigidBodyData& body1, const b3Vector3& pointInA,
+		b3RigidBodyData& body2, const b3Vector3& pointInB,
+		int limit_index,
+		const b3Vector3& axis_normal_on_a,
+		const b3Vector3& anchorPos);
+};
+
+enum b36DofFlags
+{
+	B3_6DOF_FLAGS_CFM_NORM = 1,
+	B3_6DOF_FLAGS_CFM_STOP = 2,
+	B3_6DOF_FLAGS_ERP_STOP = 4
+};
+#define B3_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
+
+/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/*!
+b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+currently this limit supports rotational motors<br>
+<ul>
+<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
+At this moment translational motors are not supported. May be in the future. </li>
+
+<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
+This is accessible through b3Generic6DofConstraint.getLimitMotor method,
+This brings support for limit parameters and motors. </li>
+
+<li> Angulars limits have these possible ranges:
+<table border=1 >
+<tr>
+	<td><b>AXIS</b></td>
+	<td><b>MIN ANGLE</b></td>
+	<td><b>MAX ANGLE</b></td>
+</tr><tr>
+	<td>X</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr><tr>
+	<td>Y</td>
+	<td>-PI/2</td>
+	<td>PI/2</td>
+</tr><tr>
+	<td>Z</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr>
+</table>
+</li>
+</ul>
+
+*/
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Generic6DofConstraint : public b3TypedConstraint
+{
+protected:
+	//! relative_frames
+	//!@{
+	b3Transform m_frameInA;  //!< the constraint space w.r.t body A
+	b3Transform m_frameInB;  //!< the constraint space w.r.t body B
+	//!@}
+
+	//! Jacobians
+	//!@{
+	//    b3JacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
+	//    b3JacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
+	//!@}
+
+	//! Linear_Limit_parameters
+	//!@{
+	b3TranslationalLimitMotor m_linearLimits;
+	//!@}
+
+	//! hinge_parameters
+	//!@{
+	b3RotationalLimitMotor m_angularLimits[3];
+	//!@}
+
+protected:
+	//! temporal variables
+	//!@{
+	b3Transform m_calculatedTransformA;
+	b3Transform m_calculatedTransformB;
+	b3Vector3 m_calculatedAxisAngleDiff;
+	b3Vector3 m_calculatedAxis[3];
+	b3Vector3 m_calculatedLinearDiff;
+	b3Scalar m_timeStep;
+	b3Scalar m_factA;
+	b3Scalar m_factB;
+	bool m_hasStaticBody;
+
+	b3Vector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
+
+	bool m_useLinearReferenceFrameA;
+	bool m_useOffsetForConstraintFrame;
+
+	int m_flags;
+
+	//!@}
+
+	b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
+	{
+		b3Assert(0);
+		(void)other;
+		return *this;
+	}
+
+	int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
+
+	int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
+
+	// tests linear limits
+	void calculateLinearInfo();
+
+	//! calcs the euler angles between the two bodies.
+	void calculateAngleInfo();
+
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
+
+	//! Calcs global transform of the offsets
+	/*!
+	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
+	\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
+	*/
+	void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
+
+	void calculateTransforms(const b3RigidBodyData* bodies);
+
+	//! Gets the global transform of the offset for body A
+	/*!
+    \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
+    */
+	const b3Transform& getCalculatedTransformA() const
+	{
+		return m_calculatedTransformA;
+	}
+
+	//! Gets the global transform of the offset for body B
+	/*!
+    \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
+    */
+	const b3Transform& getCalculatedTransformB() const
+	{
+		return m_calculatedTransformB;
+	}
+
+	const b3Transform& getFrameOffsetA() const
+	{
+		return m_frameInA;
+	}
+
+	const b3Transform& getFrameOffsetB() const
+	{
+		return m_frameInB;
+	}
+
+	b3Transform& getFrameOffsetA()
+	{
+		return m_frameInA;
+	}
+
+	b3Transform& getFrameOffsetB()
+	{
+		return m_frameInB;
+	}
+
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
+
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
+
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
+
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
+
+	void updateRHS(b3Scalar timeStep);
+
+	//! Get the rotation axis in global coordinates
+	b3Vector3 getAxis(int axis_index) const;
+
+	//! Get the relative Euler angle
+	/*!
+	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
+	*/
+	b3Scalar getAngle(int axis_index) const;
+
+	//! Get the relative position of the constraint pivot
+	/*!
+	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
+	*/
+	b3Scalar getRelativePivotPosition(int axis_index) const;
+
+	void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
+
+	//! Test angular limit.
+	/*!
+	Calculates angular correction and returns true if limit needs to be corrected.
+	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
+	*/
+	bool testAngularLimitMotor(int axis_index);
+
+	void setLinearLowerLimit(const b3Vector3& linearLower)
+	{
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
+
+	void getLinearLowerLimit(b3Vector3 & linearLower)
+	{
+		linearLower = m_linearLimits.m_lowerLimit;
+	}
+
+	void setLinearUpperLimit(const b3Vector3& linearUpper)
+	{
+		m_linearLimits.m_upperLimit = linearUpper;
+	}
+
+	void getLinearUpperLimit(b3Vector3 & linearUpper)
+	{
+		linearUpper = m_linearLimits.m_upperLimit;
+	}
+
+	void setAngularLowerLimit(const b3Vector3& angularLower)
+	{
+		for (int i = 0; i < 3; i++)
+			m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
+	}
+
+	void getAngularLowerLimit(b3Vector3 & angularLower)
+	{
+		for (int i = 0; i < 3; i++)
+			angularLower[i] = m_angularLimits[i].m_loLimit;
+	}
+
+	void setAngularUpperLimit(const b3Vector3& angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
+	}
+
+	void getAngularUpperLimit(b3Vector3 & angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
+			angularUpper[i] = m_angularLimits[i].m_hiLimit;
+	}
+
+	//! Retrieves the angular limit informacion
+	b3RotationalLimitMotor* getRotationalLimitMotor(int index)
+	{
+		return &m_angularLimits[index];
+	}
+
+	//! Retrieves the  limit informacion
+	b3TranslationalLimitMotor* getTranslationalLimitMotor()
+	{
+		return &m_linearLimits;
+	}
+
+	//first 3 are linear, next 3 are angular
+	void setLimit(int axis, b3Scalar lo, b3Scalar hi)
+	{
+		if (axis < 3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = b3NormalizeAngle(lo);
+			hi = b3NormalizeAngle(hi);
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
+
+	//! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+	bool isLimited(int limitIndex)
+	{
+		if (limitIndex < 3)
+		{
+			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
+
+	virtual void calcAnchorPos(const b3RigidBodyData* bodies);  // overridable
+
+	int get_limit_motor_info2(b3RotationalLimitMotor * limot,
+							  const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+							  b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
+
+	// access for UseFrameOffset
+	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
+	///return the local value of parameter
+	virtual b3Scalar getParam(int num, int axis = -1) const;
+
+	void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
+};
+
+#endif  //B3_GENERIC_6DOF_CONSTRAINT_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h

@@ -0,0 +1,150 @@
+/*
+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 B3_JACOBIAN_ENTRY_H
+#define B3_JACOBIAN_ENTRY_H
+
+#include "Bullet3Common/b3Matrix3x3.h"
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the b3JacobianEntry memory layout 16 bytes
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/// Jacobian entry is an abstraction that allows to describe constraints
+/// it can be used in combination with a constraint solver
+/// Can be used to relate the effect of an impulse to the constraint error
+B3_ATTRIBUTE_ALIGNED16(class)
+b3JacobianEntry
+{
+public:
+	b3JacobianEntry(){};
+	//constraint between two different rigidbodies
+	b3JacobianEntry(
+		const b3Matrix3x3& world2A,
+		const b3Matrix3x3& world2B,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+		const b3Vector3& jointAxis,
+		const b3Vector3& inertiaInvA,
+		const b3Scalar massInvA,
+		const b3Vector3& inertiaInvB,
+		const b3Scalar massInvB)
+		: m_linearJointAxis(jointAxis)
+	{
+		m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	b3JacobianEntry(const b3Vector3& jointAxis,
+					const b3Matrix3x3& world2A,
+					const b3Matrix3x3& world2B,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
+	{
+		m_aJ = world2A * jointAxis;
+		m_bJ = world2B * -jointAxis;
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	//angular constraint between two different rigidbodies
+	b3JacobianEntry(const b3Vector3& axisInA,
+					const b3Vector3& axisInB,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
+	{
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	//constraint on one rigidbody
+	b3JacobianEntry(
+		const b3Matrix3x3& world2A,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+		const b3Vector3& jointAxis,
+		const b3Vector3& inertiaInvA,
+		const b3Scalar massInvA)
+		: m_linearJointAxis(jointAxis)
+	{
+		m_aJ = world2A * (rel_pos1.cross(jointAxis));
+		m_bJ = world2A * (rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	b3Scalar getDiagonal() const { return m_Adiag; }
+
+	// for two constraints on the same rigidbody (for example vehicle friction)
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
+	{
+		const b3JacobianEntry& jacA = *this;
+		b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+		b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+		return lin + ang;
+	}
+
+	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
+	{
+		const b3JacobianEntry& jacA = *this;
+		b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
+		b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+		b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
+		b3Vector3 lin0 = massInvA * lin;
+		b3Vector3 lin1 = massInvB * lin;
+		b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
+	}
+
+	b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
+	{
+		b3Vector3 linrel = linvelA - linvelB;
+		b3Vector3 angvela = angvelA * m_aJ;
+		b3Vector3 angvelb = angvelB * m_bJ;
+		linrel *= m_linearJointAxis;
+		angvela += angvelb;
+		angvela += linrel;
+		b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
+		return rel_vel2 + B3_EPSILON;
+	}
+	//private:
+
+	b3Vector3 m_linearJointAxis;
+	b3Vector3 m_aJ;
+	b3Vector3 m_bJ;
+	b3Vector3 m_0MinvJt;
+	b3Vector3 m_1MinvJt;
+	//Optimization: can be stored in the w/last component of one of the vectors
+	b3Scalar m_Adiag;
+};
+
+#endif  //B3_JACOBIAN_ENTRY_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp

@@ -0,0 +1,1696 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2012 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.
+*/
+
+//enable B3_SOLVER_DEBUG if you experience solver crashes
+//#define B3_SOLVER_DEBUG
+//#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+
+//#define DISABLE_JOINTS
+
+#include "b3PgsJacobiSolver.h"
+#include "Bullet3Common/b3MinMax.h"
+#include "b3TypedConstraint.h"
+#include <new>
+#include "Bullet3Common/b3StackAlloc.h"
+
+//#include "b3SolverBody.h"
+//#include "b3SolverConstraint.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include <string.h>  //for memset
+//#include "../../dynamics/basic_demo/Stubs/AdlContact4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+static b3Transform getWorldTransform(b3RigidBodyData* rb)
+{
+	b3Transform newTrans;
+	newTrans.setOrigin(rb->m_pos);
+	newTrans.setRotation(rb->m_quat);
+	return newTrans;
+}
+
+static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia)
+{
+	return inertia->m_invInertiaWorld;
+}
+
+static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb)
+{
+	return rb->m_linVel;
+}
+
+static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb)
+{
+	return rb->m_angVel;
+}
+
+static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos)
+{
+	//we also calculate lin/ang velocity for kinematic objects
+	return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
+}
+
+struct b3ContactPoint
+{
+	b3Vector3 m_positionWorldOnA;
+	b3Vector3 m_positionWorldOnB;
+	b3Vector3 m_normalWorldOnB;
+	b3Scalar m_appliedImpulse;
+	b3Scalar m_distance;
+	b3Scalar m_combinedRestitution;
+
+	///information related to friction
+	b3Scalar m_combinedFriction;
+	b3Vector3 m_lateralFrictionDir1;
+	b3Vector3 m_lateralFrictionDir2;
+	b3Scalar m_appliedImpulseLateral1;
+	b3Scalar m_appliedImpulseLateral2;
+	b3Scalar m_combinedRollingFriction;
+	b3Scalar m_contactMotion1;
+	b3Scalar m_contactMotion2;
+	b3Scalar m_contactCFM1;
+	b3Scalar m_contactCFM2;
+
+	bool m_lateralFrictionInitialized;
+
+	b3Vector3 getPositionWorldOnA()
+	{
+		return m_positionWorldOnA;
+	}
+	b3Vector3 getPositionWorldOnB()
+	{
+		return m_positionWorldOnB;
+	}
+	b3Scalar getDistance()
+	{
+		return m_distance;
+	}
+};
+
+void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
+{
+	pointOut.m_appliedImpulse = 0.f;
+	pointOut.m_appliedImpulseLateral1 = 0.f;
+	pointOut.m_appliedImpulseLateral2 = 0.f;
+	pointOut.m_combinedFriction = contact->getFrictionCoeff();
+	pointOut.m_combinedRestitution = contact->getRestituitionCoeff();
+	pointOut.m_combinedRollingFriction = 0.f;
+	pointOut.m_contactCFM1 = 0.f;
+	pointOut.m_contactCFM2 = 0.f;
+	pointOut.m_contactMotion1 = 0.f;
+	pointOut.m_contactMotion2 = 0.f;
+	pointOut.m_distance = contact->getPenetration(contactIndex);  //??0.01f
+	b3Vector3 normalOnB = contact->m_worldNormalOnB;
+	normalOnB.normalize();  //is this needed?
+
+	b3Vector3 l1, l2;
+	b3PlaneSpace1(normalOnB, l1, l2);
+
+	pointOut.m_normalWorldOnB = normalOnB;
+	//printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ());
+	pointOut.m_lateralFrictionDir1 = l1;
+	pointOut.m_lateralFrictionDir2 = l2;
+	pointOut.m_lateralFrictionInitialized = true;
+
+	b3Vector3 worldPosB = contact->m_worldPosB[contactIndex];
+	pointOut.m_positionWorldOnB = worldPosB;
+	pointOut.m_positionWorldOnA = worldPosB + normalOnB * pointOut.m_distance;
+}
+
+int getNumContacts(b3Contact4* contact)
+{
+	return contact->getNPoints();
+}
+
+b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs)
+	: m_usePgs(usePgs),
+	  m_numSplitImpulseRecoveries(0),
+	  m_btSeed2(0)
+{
+}
+
+b3PgsJacobiSolver::~b3PgsJacobiSolver()
+{
+}
+
+void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
+{
+	b3ContactSolverInfo infoGlobal;
+	infoGlobal.m_splitImpulse = false;
+	infoGlobal.m_timeStep = 1.f / 60.f;
+	infoGlobal.m_numIterations = 4;  //4;
+									 //	infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
+	//infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
+	infoGlobal.m_solverMode |= B3_SOLVER_USE_2_FRICTION_DIRECTIONS;
+
+	//if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+	//if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+
+	solveGroup(bodies, inertias, numBodies, contacts, numContacts, constraints, numConstraints, infoGlobal);
+
+	if (!numContacts)
+		return;
+}
+
+/// b3PgsJacobiSolver Sequentially applies impulses
+b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies,
+									   b3InertiaData* inertias,
+									   int numBodies,
+									   b3Contact4* manifoldPtr,
+									   int numManifolds,
+									   b3TypedConstraint** constraints,
+									   int numConstraints,
+									   const b3ContactSolverInfo& infoGlobal)
+{
+	B3_PROFILE("solveGroup");
+	//you need to provide at least some bodies
+
+	solveGroupCacheFriendlySetup(bodies, inertias, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal);
+
+	solveGroupCacheFriendlyIterations(constraints, numConstraints, infoGlobal);
+
+	solveGroupCacheFriendlyFinish(bodies, inertias, numBodies, infoGlobal);
+
+	return 0.f;
+}
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
+static inline __m128 b3SimdDot3(__m128 vec0, __m128 vec1)
+{
+	__m128 result = _mm_mul_ps(vec0, vec1);
+	return _mm_add_ps(b3VecSplat(result, 0), _mm_add_ps(b3VecSplat(result, 1), b3VecSplat(result, 2)));
+}
+#endif  //USE_SIMD
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+#else
+	resolveSingleConstraintRowGeneric(body1, body2, c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
+{
+	b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm;
+	const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+
+	//	const b3Scalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+
+	const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else if (sum > c.m_upperLimit)
+	{
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_upperLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+
+	body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+}
+
+void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+#else
+	resolveSingleConstraintRowLowerLimit(body1, body2, c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
+{
+	b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm;
+	const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+	const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+	body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+}
+
+void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
+	b3SolverBody& body1,
+	b3SolverBody& body2,
+	const b3SolverConstraint& c)
+{
+	if (c.m_rhsPenetration)
+	{
+		m_numSplitImpulseRecoveries++;
+		b3Scalar deltaImpulse = c.m_rhsPenetration - b3Scalar(c.m_appliedPushImpulse) * c.m_cfm;
+		const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
+		const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
+
+		deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+		deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+		const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse;
+		if (sum < c.m_lowerLimit)
+		{
+			deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
+			c.m_appliedPushImpulse = c.m_lowerLimit;
+		}
+		else
+		{
+			c.m_appliedPushImpulse = sum;
+		}
+		body1.internalApplyPushImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+		body2.internalApplyPushImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	}
+}
+
+void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
+{
+#ifdef USE_SIMD
+	if (!c.m_rhsPenetration)
+		return;
+
+	m_numSplitImpulseRecoveries++;
+
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetTurnVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetTurnVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetPushVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+#else
+	resolveSplitPenetrationImpulseCacheFriendly(body1, body2, c);
+#endif
+}
+
+unsigned long b3PgsJacobiSolver::b3Rand2()
+{
+	m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
+	return m_btSeed2;
+}
+
+//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
+int b3PgsJacobiSolver::b3RandInt2(int n)
+{
+	// seems good; xor-fold and modulus
+	const unsigned long un = static_cast<unsigned long>(n);
+	unsigned long r = b3Rand2();
+
+	// note: probably more aggressive than it needs to be -- might be
+	//       able to get away without one or two of the innermost branches.
+	if (un <= 0x00010000UL)
+	{
+		r ^= (r >> 16);
+		if (un <= 0x00000100UL)
+		{
+			r ^= (r >> 8);
+			if (un <= 0x00000010UL)
+			{
+				r ^= (r >> 4);
+				if (un <= 0x00000004UL)
+				{
+					r ^= (r >> 2);
+					if (un <= 0x00000002UL)
+					{
+						r ^= (r >> 1);
+					}
+				}
+			}
+		}
+	}
+
+	return (int)(r % un);
+}
+
+void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb)
+{
+	solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f);
+	solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+
+	if (rb)
+	{
+		solverBody->m_worldTransform = getWorldTransform(rb);
+		solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass));
+		solverBody->m_originalBodyIndex = bodyIndex;
+		solverBody->m_angularFactor = b3MakeVector3(1, 1, 1);
+		solverBody->m_linearFactor = b3MakeVector3(1, 1, 1);
+		solverBody->m_linearVelocity = getLinearVelocity(rb);
+		solverBody->m_angularVelocity = getAngularVelocity(rb);
+	}
+	else
+	{
+		solverBody->m_worldTransform.setIdentity();
+		solverBody->internalSetInvMass(b3MakeVector3(0, 0, 0));
+		solverBody->m_originalBodyIndex = bodyIndex;
+		solverBody->m_angularFactor.setValue(1, 1, 1);
+		solverBody->m_linearFactor.setValue(1, 1, 1);
+		solverBody->m_linearVelocity.setValue(0, 0, 0);
+		solverBody->m_angularVelocity.setValue(0, 0, 0);
+	}
+}
+
+b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitution)
+{
+	b3Scalar rest = restitution * -rel_vel;
+	return rest;
+}
+
+void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+{
+	solverConstraint.m_contactNormal = normalAxis;
+	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+
+	b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex];
+	b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex];
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+	solverConstraint.m_friction = cp.m_combinedFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		b3Vector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
+	}
+	{
+		b3Vector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
+	}
+
+	b3Scalar scaledDenom;
+
+	{
+		b3Vector3 vec;
+		b3Scalar denom0 = 0.f;
+		b3Scalar denom1 = 0.f;
+		if (body0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = body0->m_invMass + normalAxis.dot(vec);
+		}
+		if (body1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = body1->m_invMass + normalAxis.dot(vec);
+		}
+
+		b3Scalar denom;
+		if (m_usePgs)
+		{
+			scaledDenom = denom = relaxation / (denom0 + denom1);
+		}
+		else
+		{
+			denom = relaxation / (denom0 + denom1);
+			b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]) : 1.f;
+			b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]) : 1.f;
+
+			scaledDenom = relaxation / (denom0 * countA + denom1 * countB);
+		}
+
+		solverConstraint.m_jacDiagABInv = denom;
+	}
+
+	{
+		b3Scalar rel_vel;
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
+
+		rel_vel = vel1Dotn + vel2Dotn;
+
+		//		b3Scalar positionalError = 0.f;
+
+		b3SimdScalar velocityError = desiredVelocity - rel_vel;
+		b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);  //solverConstraint.m_jacDiagABInv);
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_cfm = cfmSlip;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+}
+
+b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+{
+	b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+													   b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+													   b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+													   b3Scalar desiredVelocity, b3Scalar cfmSlip)
+
+{
+	b3Vector3 normalAxis = b3MakeVector3(0, 0, 0);
+
+	solverConstraint.m_contactNormal = normalAxis;
+	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+
+	b3RigidBodyData* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex];
+	b3RigidBodyData* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex];
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		b3Vector3 ftorqueAxis1 = -normalAxis1;
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
+	}
+	{
+		b3Vector3 ftorqueAxis1 = normalAxis1;
+		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
+	}
+
+	{
+		b3Vector3 iMJaA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * solverConstraint.m_relpos1CrossNormal : b3MakeVector3(0, 0, 0);
+		b3Vector3 iMJaB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * solverConstraint.m_relpos2CrossNormal : b3MakeVector3(0, 0, 0);
+		b3Scalar sum = 0;
+		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+		solverConstraint.m_jacDiagABInv = b3Scalar(1.) / sum;
+	}
+
+	{
+		b3Scalar rel_vel;
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
+
+		rel_vel = vel1Dotn + vel2Dotn;
+
+		//		b3Scalar positionalError = 0.f;
+
+		b3SimdScalar velocityError = desiredVelocity - rel_vel;
+		b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv);
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_cfm = cfmSlip;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+}
+
+b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+{
+	b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupRollingFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+								   colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias)
+{
+	//b3Assert(bodyIndex< m_tmpSolverBodyPool.size());
+
+	b3RigidBodyData& body = bodies[bodyIndex];
+	int curIndex = -1;
+	if (m_usePgs || body.m_invMass == 0.f)
+	{
+		if (m_bodyCount[bodyIndex] < 0)
+		{
+			curIndex = m_tmpSolverBodyPool.size();
+			b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(bodyIndex, &solverBody, &body);
+			solverBody.m_originalBodyIndex = bodyIndex;
+			m_bodyCount[bodyIndex] = curIndex;
+		}
+		else
+		{
+			curIndex = m_bodyCount[bodyIndex];
+		}
+	}
+	else
+	{
+		b3Assert(m_bodyCount[bodyIndex] > 0);
+		m_bodyCountCheck[bodyIndex]++;
+		curIndex = m_tmpSolverBodyPool.size();
+		b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
+		initSolverBody(bodyIndex, &solverBody, &body);
+		solverBody.m_originalBodyIndex = bodyIndex;
+	}
+
+	b3Assert(curIndex >= 0);
+	return curIndex;
+}
+#include <stdio.h>
+
+void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
+											   int solverBodyIdA, int solverBodyIdB,
+											   b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
+											   b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
+											   b3Vector3& rel_pos1, b3Vector3& rel_pos2)
+{
+	const b3Vector3& pos1 = cp.getPositionWorldOnA();
+	const b3Vector3& pos2 = cp.getPositionWorldOnB();
+
+	b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex];
+	b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex];
+
+	//			b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+	//			b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+	rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+	relaxation = 1.f;
+
+	b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex]) * torqueAxis0 : b3MakeVector3(0, 0, 0);
+	b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex]) * -torqueAxis1 : b3MakeVector3(0, 0, 0);
+
+	b3Scalar scaledDenom;
+	{
+#ifdef COMPUTE_IMPULSE_DENOM
+		b3Scalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
+		b3Scalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
+#else
+		b3Vector3 vec;
+		b3Scalar denom0 = 0.f;
+		b3Scalar denom1 = 0.f;
+		if (rb0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec);
+		}
+		if (rb1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec);
+		}
+#endif  //COMPUTE_IMPULSE_DENOM
+
+		b3Scalar denom;
+		if (m_usePgs)
+		{
+			scaledDenom = denom = relaxation / (denom0 + denom1);
+		}
+		else
+		{
+			denom = relaxation / (denom0 + denom1);
+
+			b3Scalar countA = rb0->m_invMass ? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f;
+			b3Scalar countB = rb1->m_invMass ? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f;
+			scaledDenom = relaxation / (denom0 * countA + denom1 * countB);
+		}
+		solverConstraint.m_jacDiagABInv = denom;
+	}
+
+	solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+	solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+	solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+
+	b3Scalar restitution = 0.f;
+	b3Scalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
+
+	{
+		b3Vector3 vel1, vel2;
+
+		vel1 = rb0 ? getVelocityInLocalPoint(rb0, rel_pos1) : b3MakeVector3(0, 0, 0);
+		vel2 = rb1 ? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0, 0, 0);
+
+		//			b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0);
+		vel = vel1 - vel2;
+		rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+		solverConstraint.m_friction = cp.m_combinedFriction;
+
+		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
+		if (restitution <= b3Scalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
+
+	///warm starting (or zero if disabled)
+	if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (rb0)
+			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+		if (rb1)
+			bodyB->internalApplyImpulse(solverConstraint.m_contactNormal * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(b3Scalar)solverConstraint.m_appliedImpulse);
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
+
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0 ? bodyA->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? bodyA->m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1 ? bodyB->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? bodyB->m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar rel_vel = vel1Dotn + vel2Dotn;
+
+		b3Scalar positionalError = 0.f;
+		b3Scalar velocityError = restitution - rel_vel;  // * damping;
+
+		b3Scalar erp = infoGlobal.m_erp2;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
+
+		if (penetration > 0)
+		{
+			positionalError = 0;
+
+			velocityError -= penetration / infoGlobal.m_timeStep;
+		}
+		else
+		{
+			positionalError = -penetration * erp / infoGlobal.m_timeStep;
+		}
+
+		b3Scalar penetrationImpulse = positionalError * scaledDenom;  //solverConstraint.m_jacDiagABInv;
+		b3Scalar velocityImpulse = velocityError * scaledDenom;       //solverConstraint.m_jacDiagABInv;
+
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+		}
+		else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+}
+
+void b3PgsJacobiSolver::setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
+													 int solverBodyIdA, int solverBodyIdB,
+													 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
+{
+	b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	{
+		b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+		if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+		{
+			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+			if (bodies[bodyA->m_originalBodyIndex].m_invMass)
+				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
+			if (bodies[bodyB->m_originalBodyIndex].m_invMass)
+				bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint1.m_angularComponentB, -(b3Scalar)frictionConstraint1.m_appliedImpulse);
+		}
+		else
+		{
+			frictionConstraint1.m_appliedImpulse = 0.f;
+		}
+	}
+
+	if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+	{
+		b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
+		if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+		{
+			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+			if (bodies[bodyA->m_originalBodyIndex].m_invMass)
+				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse);
+			if (bodies[bodyB->m_originalBodyIndex].m_invMass)
+				bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint2.m_angularComponentB, -(b3Scalar)frictionConstraint2.m_appliedImpulse);
+		}
+		else
+		{
+			frictionConstraint2.m_appliedImpulse = 0.f;
+		}
+	}
+}
+
+void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal)
+{
+	b3RigidBodyData *colObj0 = 0, *colObj1 = 0;
+
+	int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(), bodies, inertias);
+	int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(), bodies, inertias);
+
+	//	b3RigidBody* bodyA = b3RigidBody::upcast(colObj0);
+	//	b3RigidBody* bodyB = b3RigidBody::upcast(colObj1);
+
+	b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	///avoid collision response between two static objects
+	if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero())
+		return;
+
+	int rollingFriction = 1;
+	int numContacts = getNumContacts(manifold);
+	for (int j = 0; j < numContacts; j++)
+	{
+		b3ContactPoint cp;
+		getContactPoint(manifold, j, cp);
+
+		if (cp.getDistance() <= getContactProcessingThreshold(manifold))
+		{
+			b3Vector3 rel_pos1;
+			b3Vector3 rel_pos2;
+			b3Scalar relaxation;
+			b3Scalar rel_vel;
+			b3Vector3 vel;
+
+			int frictionIndex = m_tmpSolverContactConstraintPool.size();
+			b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+			//			b3RigidBody* rb0 = b3RigidBody::upcast(colObj0);
+			//			b3RigidBody* rb1 = b3RigidBody::upcast(colObj1);
+			solverConstraint.m_solverBodyIdA = solverBodyIdA;
+			solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+			solverConstraint.m_originalContactPoint = &cp;
+
+			setupContactConstraint(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+
+			//			const b3Vector3& pos1 = cp.getPositionWorldOnA();
+			//			const b3Vector3& pos2 = cp.getPositionWorldOnB();
+
+			/////setup the friction constraints
+
+			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+
+			b3Vector3 angVelA, angVelB;
+			solverBodyA->getAngularVelocity(angVelA);
+			solverBodyB->getAngularVelocity(angVelB);
+			b3Vector3 relAngVel = angVelB - angVelA;
+
+			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
+			{
+				//only a single rollingFriction per manifold
+				rollingFriction--;
+				if (relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold)
+				{
+					relAngVel.normalize();
+					if (relAngVel.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, relAngVel, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+				}
+				else
+				{
+					addRollingFrictionConstraint(bodies, inertias, cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					b3Vector3 axis0, axis1;
+					b3PlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
+					if (axis0.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					if (axis1.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+				}
+			}
+
+			///Bullet has several options to set the friction directions
+			///By default, each contact has only a single friction direction that is recomputed automatically very frame
+			///based on the relative linear velocity.
+			///If the relative velocity it zero, it will automatically compute a friction direction.
+
+			///You can also enable two friction directions, using the B3_SOLVER_USE_2_FRICTION_DIRECTIONS.
+			///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
+			///
+			///If you choose B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
+			///
+			///The user can manually override the friction directions for certain contacts using a contact callback,
+			///and set the cp.m_lateralFrictionInitialized to true
+			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+			///this will give a conveyor belt effect
+			///
+			if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			{
+				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+				b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+				if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON)
+				{
+					cp.m_lateralFrictionDir1 *= 1.f / b3Sqrt(lat_rel_vel);
+					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+					{
+						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+						cp.m_lateralFrictionDir2.normalize();  //??
+						addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					}
+
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+				}
+				else
+				{
+					b3PlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+
+					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+					{
+						addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					}
+
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+
+					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+					{
+						cp.m_lateralFrictionInitialized = true;
+					}
+				}
+			}
+			else
+			{
+				addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);
+
+				if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+
+				setFrictionConstraintImpulse(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+			}
+		}
+	}
+}
+
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
+{
+	B3_PROFILE("solveGroupCacheFriendlySetup");
+
+	m_maxOverrideNumSolverIterations = 0;
+
+	m_tmpSolverBodyPool.resize(0);
+
+	m_bodyCount.resize(0);
+	m_bodyCount.resize(numBodies, 0);
+	m_bodyCountCheck.resize(0);
+	m_bodyCountCheck.resize(numBodies, 0);
+
+	m_deltaLinearVelocities.resize(0);
+	m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
+	m_deltaAngularVelocities.resize(0);
+	m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
+
+	//int totalBodies = 0;
+
+	for (int i = 0; i < numConstraints; i++)
+	{
+		int bodyIndexA = constraints[i]->getRigidBodyA();
+		int bodyIndexB = constraints[i]->getRigidBodyB();
+		if (m_usePgs)
+		{
+			m_bodyCount[bodyIndexA] = -1;
+			m_bodyCount[bodyIndexB] = -1;
+		}
+		else
+		{
+			//didn't implement joints with Jacobi version yet
+			b3Assert(0);
+		}
+	}
+	for (int i = 0; i < numManifolds; i++)
+	{
+		int bodyIndexA = manifoldPtr[i].getBodyA();
+		int bodyIndexB = manifoldPtr[i].getBodyB();
+		if (m_usePgs)
+		{
+			m_bodyCount[bodyIndexA] = -1;
+			m_bodyCount[bodyIndexB] = -1;
+		}
+		else
+		{
+			if (bodies[bodyIndexA].m_invMass)
+			{
+				//m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints();
+				m_bodyCount[bodyIndexA]++;
+			}
+			else
+				m_bodyCount[bodyIndexA] = -1;
+
+			if (bodies[bodyIndexB].m_invMass)
+				//	m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints();
+				m_bodyCount[bodyIndexB]++;
+			else
+				m_bodyCount[bodyIndexB] = -1;
+		}
+	}
+
+	if (1)
+	{
+		int j;
+		for (j = 0; j < numConstraints; j++)
+		{
+			b3TypedConstraint* constraint = constraints[j];
+
+			constraint->internalSetAppliedImpulse(0.0f);
+		}
+	}
+
+	//b3RigidBody* rb0=0,*rb1=0;
+	//if (1)
+	{
+		{
+			int totalNumRows = 0;
+			int i;
+
+			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+			//calculate the total number of contraint rows
+			for (i = 0; i < numConstraints; i++)
+			{
+				b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+				b3JointFeedback* fb = constraints[i]->getJointFeedback();
+				if (fb)
+				{
+					fb->m_appliedForceBodyA.setZero();
+					fb->m_appliedTorqueBodyA.setZero();
+					fb->m_appliedForceBodyB.setZero();
+					fb->m_appliedTorqueBodyB.setZero();
+				}
+
+				if (constraints[i]->isEnabled())
+				{
+				}
+				if (constraints[i]->isEnabled())
+				{
+					constraints[i]->getInfo1(&info1, bodies);
+				}
+				else
+				{
+					info1.m_numConstraintRows = 0;
+					info1.nub = 0;
+				}
+				totalNumRows += info1.m_numConstraintRows;
+			}
+			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+#ifndef DISABLE_JOINTS
+			///setup the b3SolverConstraints
+			int currentRow = 0;
+
+			for (i = 0; i < numConstraints; i++)
+			{
+				const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+				if (info1.m_numConstraintRows)
+				{
+					b3Assert(currentRow < totalNumRows);
+
+					b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+					b3TypedConstraint* constraint = constraints[i];
+
+					b3RigidBodyData& rbA = bodies[constraint->getRigidBodyA()];
+					//b3RigidBody& rbA = constraint->getRigidBodyA();
+					//				b3RigidBody& rbB = constraint->getRigidBodyB();
+					b3RigidBodyData& rbB = bodies[constraint->getRigidBodyB()];
+
+					int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(), bodies, inertias);
+					int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(), bodies, inertias);
+
+					b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+					b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
+					if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
+						m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
+
+					int j;
+					for (j = 0; j < info1.m_numConstraintRows; j++)
+					{
+						memset(&currentConstraintRow[j], 0, sizeof(b3SolverConstraint));
+						currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
+						currentConstraintRow[j].m_upperLimit = B3_INFINITY;
+						currentConstraintRow[j].m_appliedImpulse = 0.f;
+						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
+					}
+
+					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+
+					b3TypedConstraint::b3ConstraintInfo2 info2;
+					info2.fps = 1.f / infoGlobal.m_timeStep;
+					info2.erp = infoGlobal.m_erp;
+					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
+					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+					info2.m_J2linearAxis = 0;
+					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+					info2.rowskip = sizeof(b3SolverConstraint) / sizeof(b3Scalar);  //check this
+																					///the size of b3SolverConstraint needs be a multiple of b3Scalar
+					b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3SolverConstraint));
+					info2.m_constraintError = &currentConstraintRow->m_rhs;
+					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+					info2.m_damping = infoGlobal.m_damping;
+					info2.cfm = &currentConstraintRow->m_cfm;
+					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					info2.m_numIterations = infoGlobal.m_numIterations;
+					constraints[i]->getInfo2(&info2, bodies);
+
+					///finalize the constraint setup
+					for (j = 0; j < info1.m_numConstraintRows; j++)
+					{
+						b3SolverConstraint& solverConstraint = currentConstraintRow[j];
+
+						if (solverConstraint.m_upperLimit >= constraints[i]->getBreakingImpulseThreshold())
+						{
+							solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
+						}
+
+						if (solverConstraint.m_lowerLimit <= -constraints[i]->getBreakingImpulseThreshold())
+						{
+							solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
+						}
+
+						solverConstraint.m_originalContactPoint = constraint;
+
+						b3Matrix3x3& invInertiaWorldA = inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
+						{
+							//b3Vector3 angularFactorA(1,1,1);
+							const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+							solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1;  //*angularFactorA;
+						}
+
+						b3Matrix3x3& invInertiaWorldB = inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
+						{
+							const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+							solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2;  //*constraint->getRigidBodyB().getAngularFactor();
+						}
+
+						{
+							//it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
+							//because it gets multiplied iMJlB
+							b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass;
+							b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal;
+							b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass;  //sign of normal?
+							b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal;
+
+							b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+							sum += iMJlB.dot(solverConstraint.m_contactNormal);
+							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+							b3Scalar fsum = b3Fabs(sum);
+							b3Assert(fsum > B3_EPSILON);
+							solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f;
+						}
+
+						///fix rhs
+						///todo: add force/torque accelerators
+						{
+							b3Scalar rel_vel;
+							b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel);
+							b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel);
+
+							rel_vel = vel1Dotn + vel2Dotn;
+
+							b3Scalar restitution = 0.f;
+							b3Scalar positionalError = solverConstraint.m_rhs;  //already filled in by getConstraintInfo2
+							b3Scalar velocityError = restitution - rel_vel * info2.m_damping;
+							b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+							b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+							solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+							solverConstraint.m_appliedImpulse = 0.f;
+						}
+					}
+				}
+				currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
+			}
+#endif  //DISABLE_JOINTS
+		}
+
+		{
+			int i;
+
+			for (i = 0; i < numManifolds; i++)
+			{
+				b3Contact4& manifold = manifoldPtr[i];
+				convertContact(bodies, inertias, &manifold, infoGlobal);
+			}
+		}
+	}
+
+	//	b3ContactSolverInfo info = infoGlobal;
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
+	if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
+	else
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
+	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
+	{
+		int i;
+		for (i = 0; i < numNonContactPool; i++)
+		{
+			m_orderNonContactConstraintPool[i] = i;
+		}
+		for (i = 0; i < numConstraintPool; i++)
+		{
+			m_orderTmpConstraintPool[i] = i;
+		}
+		for (i = 0; i < numFrictionPool; i++)
+		{
+			m_orderFrictionConstraintPool[i] = i;
+		}
+	}
+
+	return 0.f;
+}
+
+b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
+{
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
+				int tmp = m_orderNonContactConstraintPool[j];
+				int swapi = b3RandInt2(j + 1);
+				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
+				m_orderNonContactConstraintPool[swapi] = tmp;
+			}
+
+			//contact/friction constraints are not solved more than
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
+					int tmp = m_orderTmpConstraintPool[j];
+					int swapi = b3RandInt2(j + 1);
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[swapi] = tmp;
+				}
+
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
+					int tmp = m_orderFrictionConstraintPool[j];
+					int swapi = b3RandInt2(j + 1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+	if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
+	{
+		///solve all joint constraints, using SIMD, if available
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
+		{
+			b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations)
+				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
+		}
+
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
+
+				for (int c = 0; c < numPoolConstraints; c++)
+				{
+					b3Scalar totalImpulse = 0;
+
+					{
+						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
+
+							if (totalImpulse > b3Scalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							}
+						}
+
+						if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
+
+							if (totalImpulse > b3Scalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							}
+						}
+					}
+				}
+			}
+			else  //B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
+				for (j = 0; j < numPoolConstraints; j++)
+				{
+					const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+				}
+
+				if (!m_usePgs)
+					averageVelocities();
+
+				///solve all friction constraints, using SIMD, if available
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j = 0; j < numFrictionPoolConstraints; j++)
+				{
+					b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse > b3Scalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					}
+				}
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j = 0; j < numRollingFrictionPoolConstraints; j++)
+				{
+					b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse > b3Scalar(0))
+					{
+						b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+						if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		//non-SIMD version
+		///solve all joint constraints
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
+		{
+			b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations)
+				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
+		}
+
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			for (int j = 0; j < numPoolConstraints; j++)
+			{
+				const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			for (int j = 0; j < numFrictionPoolConstraints; j++)
+			{
+				b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+				if (totalImpulse > b3Scalar(0))
+				{
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
+			{
+				b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse > b3Scalar(0))
+				{
+					b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				}
+			}
+		}
+	}
+	return 0.f;
+}
+
+void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
+{
+	int iteration;
+	if (infoGlobal.m_splitImpulse)
+	{
+		if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
+		{
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j = 0; j < numPoolConstraints; j++)
+					{
+						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					}
+				}
+			}
+		}
+		else
+		{
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j = 0; j < numPoolConstraints; j++)
+					{
+						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					}
+				}
+			}
+		}
+	}
+}
+
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
+{
+	B3_PROFILE("solveGroupCacheFriendlyIterations");
+
+	{
+		///this is a special step to resolve penetrations (just for contacts)
+		solveGroupCacheFriendlySplitImpulseIterations(constraints, numConstraints, infoGlobal);
+
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
+		{
+			solveSingleIteration(iteration, constraints, numConstraints, infoGlobal);
+
+			if (!m_usePgs)
+			{
+				averageVelocities();
+			}
+		}
+	}
+	return 0.f;
+}
+
+void b3PgsJacobiSolver::averageVelocities()
+{
+	B3_PROFILE("averaging");
+	//average the velocities
+	int numBodies = m_bodyCount.size();
+
+	m_deltaLinearVelocities.resize(0);
+	m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
+	m_deltaAngularVelocities.resize(0);
+	m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
+
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+	{
+		if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
+		{
+			int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
+			m_deltaLinearVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaLinearVelocity();
+			m_deltaAngularVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaAngularVelocity();
+		}
+	}
+
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+	{
+		int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
+
+		if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
+		{
+			b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
+
+			b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]);
+
+			m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor;
+			m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor;
+		}
+	}
+}
+
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal)
+{
+	B3_PROFILE("solveGroupCacheFriendlyFinish");
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+	int i, j;
+
+	if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+	{
+		for (j = 0; j < numPoolConstraints; j++)
+		{
+			const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+			b3ContactPoint* pt = (b3ContactPoint*)solveManifold.m_originalContactPoint;
+			b3Assert(pt);
+			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+			//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
+			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
+			}
+			//do a callback here?
+		}
+	}
+
+	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
+	for (j = 0; j < numPoolConstraints; j++)
+	{
+		const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+		b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint;
+		b3JointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA];
+			b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdB];
+
+			fb->m_appliedForceBodyA += solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyA->m_linearFactor / infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += -solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyB->m_linearFactor / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * bodyA->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal * bodyB->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
+		}
+
+		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
+		if (b3Fabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
+		{
+			constr->setEnabled(false);
+		}
+	}
+
+	{
+		B3_PROFILE("write back velocities and transforms");
+		for (i = 0; i < m_tmpSolverBodyPool.size(); i++)
+		{
+			int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
+			//b3Assert(i==bodyIndex);
+
+			b3RigidBodyData* body = &bodies[bodyIndex];
+			if (body->m_invMass)
+			{
+				if (infoGlobal.m_splitImpulse)
+					m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
+				else
+					m_tmpSolverBodyPool[i].writebackVelocity();
+
+				if (m_usePgs)
+				{
+					body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
+					body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
+				}
+				else
+				{
+					b3Scalar factor = 1.f / b3Scalar(m_bodyCount[bodyIndex]);
+
+					b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex] * factor;
+					b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex] * factor;
+					//printf("body %d\n",bodyIndex);
+					//printf("deltaLinVel = %f,%f,%f\n",deltaLinVel.getX(),deltaLinVel.getY(),deltaLinVel.getZ());
+					//printf("deltaAngVel = %f,%f,%f\n",deltaAngVel.getX(),deltaAngVel.getY(),deltaAngVel.getZ());
+
+					body->m_linVel += deltaLinVel;
+					body->m_angVel += deltaAngVel;
+				}
+
+				if (infoGlobal.m_splitImpulse)
+				{
+					body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin();
+					b3Quaternion orn;
+					orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation();
+					body->m_quat = orn;
+				}
+			}
+		}
+	}
+
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	return 0.f;
+}
+
+void b3PgsJacobiSolver::reset()
+{
+	m_btSeed2 = 0;
+}

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h

@@ -0,0 +1,133 @@
+#ifndef B3_PGS_JACOBI_SOLVER
+#define B3_PGS_JACOBI_SOLVER
+
+struct b3Contact4;
+struct b3ContactPoint;
+
+class b3Dispatcher;
+
+#include "b3TypedConstraint.h"
+#include "b3ContactSolverInfo.h"
+#include "b3SolverBody.h"
+#include "b3SolverConstraint.h"
+
+struct b3RigidBodyData;
+struct b3InertiaData;
+
+class b3PgsJacobiSolver
+{
+protected:
+	b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
+	b3ConstraintArray m_tmpSolverContactConstraintPool;
+	b3ConstraintArray m_tmpSolverNonContactConstraintPool;
+	b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	b3AlignedObjectArray<int> m_orderTmpConstraintPool;
+	b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
+	b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
+	b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
+
+	b3AlignedObjectArray<int> m_bodyCount;
+	b3AlignedObjectArray<int> m_bodyCountCheck;
+
+	b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
+	b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
+
+	bool m_usePgs;
+	void averageVelocities();
+
+	int m_maxOverrideNumSolverIterations;
+
+	int m_numSplitImpulseRecoveries;
+
+	b3Scalar getContactProcessingThreshold(b3Contact4* contact)
+	{
+		return 0.02f;
+	}
+	void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+								 b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+								 b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+								 b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
+
+	void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+										b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+										b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+										b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
+
+	b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
+	b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
+
+	void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
+								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
+								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
+								b3Vector3& rel_pos1, b3Vector3& rel_pos2);
+
+	void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
+									  b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
+
+	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+	unsigned long m_btSeed2;
+
+	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
+
+	void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
+
+	void resolveSplitPenetrationSIMD(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
+
+	void resolveSplitPenetrationImpulseCacheFriendly(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
+
+	//internal method
+	int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
+	void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
+
+	void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
+
+	void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
+
+	void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
+
+	void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
+
+protected:
+	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+
+	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+
+	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
+
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3PgsJacobiSolver(bool usePgs);
+	virtual ~b3PgsJacobiSolver();
+
+	//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
+	void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
+
+	b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+
+	///clear internal cached data and reset random seed
+	virtual void reset();
+
+	unsigned long b3Rand2();
+
+	int b3RandInt2(int n);
+
+	void setRandSeed(unsigned long seed)
+	{
+		m_btSeed2 = seed;
+	}
+	unsigned long getRandSeed() const
+	{
+		return m_btSeed2;
+	}
+};
+
+#endif  //B3_PGS_JACOBI_SOLVER

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.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 "b3Point2PointConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+#include <new>
+
+b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
+	: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
+{
+}
+
+/*
+b3Point2PointConstraint::b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA)
+:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
+m_useSolveConstraintObsolete(false)
+{
+	
+}
+*/
+
+void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
+{
+	getInfo1NonVirtual(info, bodies);
+}
+
+void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
+{
+	info->m_numConstraintRows = 3;
+	info->nub = 3;
+}
+
+void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+{
+	b3Transform trA;
+	trA.setIdentity();
+	trA.setOrigin(bodies[m_rbA].m_pos);
+	trA.setRotation(bodies[m_rbA].m_quat);
+
+	b3Transform trB;
+	trB.setIdentity();
+	trB.setOrigin(bodies[m_rbB].m_pos);
+	trB.setRotation(bodies[m_rbB].m_quat);
+
+	getInfo2NonVirtual(info, trA, trB);
+}
+
+void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
+{
+	//retrieve matrices
+
+	// anchor points in global coordinates with respect to body PORs.
+
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
+	//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
+
+	{
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
+		b3Vector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
+	}
+
+	if (info->m_J2linearAxis)
+	{
+		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
+	}
+
+	b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
+
+	{
+		//	b3Vector3 a2n = -a2;
+		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
+	}
+
+	// set right hand side
+	b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
+	b3Scalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+	}
+	if (m_flags & B3_P2P_FLAGS_CFM)
+	{
+		for (j = 0; j < 3; j++)
+		{
+			info->cfm[j * info->rowskip] = m_cfm;
+		}
+	}
+
+	b3Scalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
+		if (m_setting.m_impulseClamp > 0)
+		{
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
+		}
+	}
+	info->m_damping = m_setting.m_damping;
+}
+
+void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
+{
+	(void)timeStep;
+}
+
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
+void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
+{
+	if (axis != -1)
+	{
+		b3AssertConstrParams(0);
+	}
+	else
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
+				m_erp = value;
+				m_flags |= B3_P2P_FLAGS_ERP;
+				break;
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
+				m_flags |= B3_P2P_FLAGS_CFM;
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+}
+
+///return the local value of parameter
+b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
+{
+	b3Scalar retVal(B3_INFINITY);
+	if (axis != -1)
+	{
+		b3AssertConstrParams(0);
+	}
+	else
+	{
+		switch (num)
+		{
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
+				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
+				retVal = m_erp;
+				break;
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
+				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
+				retVal = m_cfm;
+				break;
+			default:
+				b3AssertConstrParams(0);
+		}
+	}
+	return retVal;
+}

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h

@@ -0,0 +1,153 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_POINT2POINTCONSTRAINT_H
+#define B3_POINT2POINTCONSTRAINT_H
+
+#include "Bullet3Common/b3Vector3.h"
+//#include "b3JacobianEntry.h"
+#include "b3TypedConstraint.h"
+
+class b3RigidBody;
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
+#else
+#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
+#endif  //B3_USE_DOUBLE_PRECISION
+
+struct b3ConstraintSetting
+{
+	b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
+							m_damping(b3Scalar(1.)),
+							m_impulseClamp(b3Scalar(0.))
+	{
+	}
+	b3Scalar m_tau;
+	b3Scalar m_damping;
+	b3Scalar m_impulseClamp;
+};
+
+enum b3Point2PointFlags
+{
+	B3_P2P_FLAGS_ERP = 1,
+	B3_P2P_FLAGS_CFM = 2
+};
+
+/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Point2PointConstraint : public b3TypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+
+	b3Vector3 m_pivotInA;
+	b3Vector3 m_pivotInB;
+
+	int m_flags;
+	b3Scalar m_erp;
+	b3Scalar m_cfm;
+
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3ConstraintSetting m_setting;
+
+	b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
+
+	//b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA);
+
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
+
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
+
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
+
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
+
+	void updateRHS(b3Scalar timeStep);
+
+	void setPivotA(const b3Vector3& pivotA)
+	{
+		m_pivotInA = pivotA;
+	}
+
+	void setPivotB(const b3Vector3& pivotB)
+	{
+		m_pivotInB = pivotB;
+	}
+
+	const b3Vector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	const b3Vector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
+	///return the local value of parameter
+	virtual b3Scalar getParam(int num, int axis = -1) const;
+
+	//	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	//	virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct b3Point2PointConstraintFloatData
+{
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3FloatData m_pivotInA;
+	b3Vector3FloatData m_pivotInB;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct b3Point2PointConstraintDoubleData
+{
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3DoubleData m_pivotInA;
+	b3Vector3DoubleData m_pivotInB;
+};
+
+/*
+B3_FORCE_INLINE	int	b3Point2PointConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(b3Point2PointConstraintData);
+
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+B3_FORCE_INLINE	const char*	b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
+{
+	b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;
+
+	b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+	m_pivotInA.serialize(p2pData->m_pivotInA);
+	m_pivotInB.serialize(p2pData->m_pivotInB);
+
+	return b3Point2PointConstraintDataName;
+}
+*/
+
+#endif  //B3_POINT2POINTCONSTRAINT_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h

@@ -0,0 +1,281 @@
+/*
+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 B3_SOLVER_BODY_H
+#define B3_SOLVER_BODY_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+
+#include "Bullet3Common/b3AlignedAllocator.h"
+#include "Bullet3Common/b3TransformUtil.h"
+
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef B3_USE_SSE
+#define USE_SIMD 1
+#endif  //
+
+#ifdef USE_SIMD
+
+struct b3SimdScalar
+{
+	B3_FORCE_INLINE b3SimdScalar()
+	{
+	}
+
+	B3_FORCE_INLINE b3SimdScalar(float fl)
+		: m_vec128(_mm_set1_ps(fl))
+	{
+	}
+
+	B3_FORCE_INLINE b3SimdScalar(__m128 v128)
+		: m_vec128(v128)
+	{
+	}
+	union {
+		__m128 m_vec128;
+		float m_floats[4];
+		float x, y, z, w;
+		int m_ints[4];
+		b3Scalar m_unusedPadding;
+	};
+	B3_FORCE_INLINE __m128 get128()
+	{
+		return m_vec128;
+	}
+
+	B3_FORCE_INLINE const __m128 get128() const
+	{
+		return m_vec128;
+	}
+
+	B3_FORCE_INLINE void set128(__m128 v128)
+	{
+		m_vec128 = v128;
+	}
+
+	B3_FORCE_INLINE operator __m128()
+	{
+		return m_vec128;
+	}
+	B3_FORCE_INLINE operator const __m128() const
+	{
+		return m_vec128;
+	}
+
+	B3_FORCE_INLINE operator float() const
+	{
+		return m_floats[0];
+	}
+};
+
+///@brief Return the elementwise product of two b3SimdScalar
+B3_FORCE_INLINE b3SimdScalar
+operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
+{
+	return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
+}
+
+///@brief Return the elementwise product of two b3SimdScalar
+B3_FORCE_INLINE b3SimdScalar
+operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
+{
+	return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
+}
+
+#else
+#define b3SimdScalar b3Scalar
+#endif
+
+///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverBody
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+	b3Transform m_worldTransform;
+	b3Vector3 m_deltaLinearVelocity;
+	b3Vector3 m_deltaAngularVelocity;
+	b3Vector3 m_angularFactor;
+	b3Vector3 m_linearFactor;
+	b3Vector3 m_invMass;
+	b3Vector3 m_pushVelocity;
+	b3Vector3 m_turnVelocity;
+	b3Vector3 m_linearVelocity;
+	b3Vector3 m_angularVelocity;
+
+	union {
+		void* m_originalBody;
+		int m_originalBodyIndex;
+	};
+
+	int padding[3];
+
+	void setWorldTransform(const b3Transform& worldTransform)
+	{
+		m_worldTransform = worldTransform;
+	}
+
+	const b3Transform& getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+
+	B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
+	{
+		if (m_originalBody)
+			velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
+		else
+			velocity.setValue(0, 0, 0);
+	}
+
+	B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
+	{
+		if (m_originalBody)
+			angVel = m_angularVelocity + m_deltaAngularVelocity;
+		else
+			angVel.setValue(0, 0, 0);
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
+		}
+	}
+
+	B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
+		}
+	}
+
+	const b3Vector3& getDeltaLinearVelocity() const
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	const b3Vector3& getDeltaAngularVelocity() const
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& getPushVelocity() const
+	{
+		return m_pushVelocity;
+	}
+
+	const b3Vector3& getTurnVelocity() const
+	{
+		return m_turnVelocity;
+	}
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+
+	b3Vector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	b3Vector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const b3Vector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+
+	void internalSetInvMass(const b3Vector3& invMass)
+	{
+		m_invMass = invMass;
+	}
+
+	b3Vector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	b3Vector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
+	{
+		velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
+	{
+		angVel = m_angularVelocity + m_deltaAngularVelocity;
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
+	{
+		//if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
+		}
+	}
+
+	void writebackVelocity()
+	{
+		//if (m_originalBody>=0)
+		{
+			m_linearVelocity += m_deltaLinearVelocity;
+			m_angularVelocity += m_deltaAngularVelocity;
+
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+	void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
+	{
+		(void)timeStep;
+		if (m_originalBody)
+		{
+			m_linearVelocity += m_deltaLinearVelocity;
+			m_angularVelocity += m_deltaAngularVelocity;
+
+			//correct the position/orientation based on push/turn recovery
+			b3Transform newTransform;
+			if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
+			{
+				//	b3Quaternion orn = m_worldTransform.getRotation();
+				b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
+				m_worldTransform = newTransform;
+			}
+			//m_worldTransform.setRotation(orn);
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+};
+
+#endif  //B3_SOLVER_BODY_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h

@@ -0,0 +1,73 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SOLVER_CONSTRAINT_H
+#define B3_SOLVER_CONSTRAINT_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+//#include "b3JacobianEntry.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+#include "b3SolverBody.h"
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverConstraint
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3 m_relpos1CrossNormal;
+	b3Vector3 m_contactNormal;
+
+	b3Vector3 m_relpos2CrossNormal;
+	//b3Vector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
+
+	b3Vector3 m_angularComponentA;
+	b3Vector3 m_angularComponentB;
+
+	mutable b3SimdScalar m_appliedPushImpulse;
+	mutable b3SimdScalar m_appliedImpulse;
+	int m_padding1;
+	int m_padding2;
+	b3Scalar m_friction;
+	b3Scalar m_jacDiagABInv;
+	b3Scalar m_rhs;
+	b3Scalar m_cfm;
+
+	b3Scalar m_lowerLimit;
+	b3Scalar m_upperLimit;
+	b3Scalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		b3Scalar m_unusedPadding4;
+	};
+
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
+	int m_solverBodyIdA;
+	int m_solverBodyIdB;
+
+	enum b3SolverConstraintType
+	{
+		B3_SOLVER_CONTACT_1D = 0,
+		B3_SOLVER_FRICTION_1D
+	};
+};
+
+typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
+
+#endif  //B3_SOLVER_CONSTRAINT_H

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp

@@ -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.
+*/
+
+#include "b3TypedConstraint.h"
+//#include "Bullet3Common/b3Serializer.h"
+
+#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
+
+b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
+	: b3TypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(B3_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(rbB),
+	  m_appliedImpulse(b3Scalar(0.)),
+	  m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
+{
+}
+
+b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
+{
+	if (lowLim > uppLim)
+	{
+		return b3Scalar(1.0f);
+	}
+	else if (lowLim == uppLim)
+	{
+		return b3Scalar(0.0f);
+	}
+	b3Scalar lim_fact = b3Scalar(1.0f);
+	b3Scalar delta_max = vel / timeFact;
+	if (delta_max < b3Scalar(0.0f))
+	{
+		if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if (pos < lowLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else if (delta_max > b3Scalar(0.0f))
+	{
+		if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if (pos > uppLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else
+	{
+		lim_fact = b3Scalar(0.0f);
+	}
+	return lim_fact;
+}
+
+void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
+{
+	m_halfRange = (high - low) / 2.0f;
+	m_center = b3NormalizeAngle(low + m_halfRange);
+	m_softness = _softness;
+	m_biasFactor = _biasFactor;
+	m_relaxationFactor = _relaxationFactor;
+}
+
+void b3AngularLimit::test(const b3Scalar angle)
+{
+	m_correction = 0.0f;
+	m_sign = 0.0f;
+	m_solveLimit = false;
+
+	if (m_halfRange >= 0.0f)
+	{
+		b3Scalar deviation = b3NormalizeAngle(angle - m_center);
+		if (deviation < -m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = -(deviation + m_halfRange);
+			m_sign = +1.0f;
+		}
+		else if (deviation > m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = m_halfRange - deviation;
+			m_sign = -1.0f;
+		}
+	}
+}
+
+b3Scalar b3AngularLimit::getError() const
+{
+	return m_correction * m_sign;
+}
+
+void b3AngularLimit::fit(b3Scalar& angle) const
+{
+	if (m_halfRange > 0.0f)
+	{
+		b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
+		if (!b3Equal(relativeAngle, m_halfRange))
+		{
+			if (relativeAngle > 0.0f)
+			{
+				angle = getHigh();
+			}
+			else
+			{
+				angle = getLow();
+			}
+		}
+	}
+}
+
+b3Scalar b3AngularLimit::getLow() const
+{
+	return b3NormalizeAngle(m_center - m_halfRange);
+}
+
+b3Scalar b3AngularLimit::getHigh() const
+{
+	return b3NormalizeAngle(m_center + m_halfRange);
+}

Engine/lib/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h

@@ -0,0 +1,469 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 B3_TYPED_CONSTRAINT_H
+#define B3_TYPED_CONSTRAINT_H
+
+#include "Bullet3Common/b3Scalar.h"
+#include "b3SolverConstraint.h"
+
+class b3Serializer;
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum b3TypedConstraintType
+{
+	B3_POINT2POINT_CONSTRAINT_TYPE = 3,
+	B3_HINGE_CONSTRAINT_TYPE,
+	B3_CONETWIST_CONSTRAINT_TYPE,
+	B3_D6_CONSTRAINT_TYPE,
+	B3_SLIDER_CONSTRAINT_TYPE,
+	B3_CONTACT_CONSTRAINT_TYPE,
+	B3_D6_SPRING_CONSTRAINT_TYPE,
+	B3_GEAR_CONSTRAINT_TYPE,
+	B3_FIXED_CONSTRAINT_TYPE,
+	B3_MAX_CONSTRAINT_TYPE
+};
+
+enum b3ConstraintParams
+{
+	B3_CONSTRAINT_ERP = 1,
+	B3_CONSTRAINT_STOP_ERP,
+	B3_CONSTRAINT_CFM,
+	B3_CONSTRAINT_STOP_CFM
+};
+
+#if 1
+#define b3AssertConstrParams(_par) b3Assert(_par)
+#else
+#define b3AssertConstrParams(_par)
+#endif
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3JointFeedback
+{
+	b3Vector3 m_appliedForceBodyA;
+	b3Vector3 m_appliedTorqueBodyA;
+	b3Vector3 m_appliedForceBodyB;
+	b3Vector3 m_appliedTorqueBodyB;
+};
+
+struct b3RigidBodyData;
+
+///TypedConstraint is the baseclass for Bullet constraints and vehicles
+B3_ATTRIBUTE_ALIGNED16(class)
+b3TypedConstraint : public b3TypedObject
+{
+	int m_userConstraintType;
+
+	union {
+		int m_userConstraintId;
+		void* m_userConstraintPtr;
+	};
+
+	b3Scalar m_breakingImpulseThreshold;
+	bool m_isEnabled;
+	bool m_needsFeedback;
+	int m_overrideNumSolverIterations;
+
+	b3TypedConstraint& operator=(b3TypedConstraint& other)
+	{
+		b3Assert(0);
+		(void)other;
+		return *this;
+	}
+
+protected:
+	int m_rbA;
+	int m_rbB;
+	b3Scalar m_appliedImpulse;
+	b3Scalar m_dbgDrawSize;
+	b3JointFeedback* m_jointFeedback;
+
+	///internal method used by the constraint solver, don't use them directly
+	b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
+
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	virtual ~b3TypedConstraint(){};
+	b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
+
+	struct b3ConstraintInfo1
+	{
+		int m_numConstraintRows, nub;
+	};
+
+	struct b3ConstraintInfo2
+	{
+		// integrator parameters: frames per second (1/stepsize), default error
+		// reduction parameter (0..1).
+		b3Scalar fps, erp;
+
+		// for the first and second body, pointers to two (linear and angular)
+		// n*3 jacobian sub matrices, stored by rows. these matrices will have
+		// been initialized to 0 on entry. if the second body is zero then the
+		// J2xx pointers may be 0.
+		b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
+
+		// elements to jump from one row to the next in J's
+		int rowskip;
+
+		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+		// "constraint force mixing" vector. c is set to zero on entry, cfm is
+		// set to a constant value (typically very small or zero) value on entry.
+		b3Scalar *m_constraintError, *cfm;
+
+		// lo and hi limits for variables (set to -/+ infinity on entry).
+		b3Scalar *m_lowerLimit, *m_upperLimit;
+
+		// findex vector for variables. see the LCP solver interface for a
+		// description of what this does. this is set to -1 on entry.
+		// note that the returned indexes are relative to the first index of
+		// the constraint.
+		int* findex;
+		// number of solver iterations
+		int m_numIterations;
+
+		//damping of the velocity
+		b3Scalar m_damping;
+	};
+
+	int getOverrideNumSolverIterations() const
+	{
+		return m_overrideNumSolverIterations;
+	}
+
+	///override the number of constraint solver iterations used to solve this constraint
+	///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
+	void setOverrideNumSolverIterations(int overideNumIterations)
+	{
+		m_overrideNumSolverIterations = overideNumIterations;
+	}
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
+	{
+		(void)ca;
+		(void)solverBodyA;
+		(void)solverBodyB;
+		(void)timeStep;
+	}
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
+
+	///internal method used by the constraint solver, don't use them directly
+	void internalSetAppliedImpulse(b3Scalar appliedImpulse)
+	{
+		m_appliedImpulse = appliedImpulse;
+	}
+	///internal method used by the constraint solver, don't use them directly
+	b3Scalar internalGetAppliedImpulse()
+	{
+		return m_appliedImpulse;
+	}
+
+	b3Scalar getBreakingImpulseThreshold() const
+	{
+		return m_breakingImpulseThreshold;
+	}
+
+	void setBreakingImpulseThreshold(b3Scalar threshold)
+	{
+		m_breakingImpulseThreshold = threshold;
+	}
+
+	bool isEnabled() const
+	{
+		return m_isEnabled;
+	}
+
+	void setEnabled(bool enabled)
+	{
+		m_isEnabled = enabled;
+	}
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
+
+	int getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	int getRigidBodyA()
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB()
+	{
+		return m_rbB;
+	}
+
+	int getUserConstraintType() const
+	{
+		return m_userConstraintType;
+	}
+
+	void setUserConstraintType(int userConstraintType)
+	{
+		m_userConstraintType = userConstraintType;
+	};
+
+	void setUserConstraintId(int uid)
+	{
+		m_userConstraintId = uid;
+	}
+
+	int getUserConstraintId() const
+	{
+		return m_userConstraintId;
+	}
+
+	void setUserConstraintPtr(void* ptr)
+	{
+		m_userConstraintPtr = ptr;
+	}
+
+	void* getUserConstraintPtr()
+	{
+		return m_userConstraintPtr;
+	}
+
+	void setJointFeedback(b3JointFeedback * jointFeedback)
+	{
+		m_jointFeedback = jointFeedback;
+	}
+
+	const b3JointFeedback* getJointFeedback() const
+	{
+		return m_jointFeedback;
+	}
+
+	b3JointFeedback* getJointFeedback()
+	{
+		return m_jointFeedback;
+	}
+
+	int getUid() const
+	{
+		return m_userConstraintId;
+	}
+
+	bool needsFeedback() const
+	{
+		return m_needsFeedback;
+	}
+
+	///enableFeedback will allow to read the applied linear and angular impulse
+	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+	void enableFeedback(bool needsFeedback)
+	{
+		m_needsFeedback = needsFeedback;
+	}
+
+	///getAppliedImpulse is an estimated total applied impulse.
+	///This feedback could be used to determine breaking constraints or playing sounds.
+	b3Scalar getAppliedImpulse() const
+	{
+		b3Assert(m_needsFeedback);
+		return m_appliedImpulse;
+	}
+
+	b3TypedConstraintType getConstraintType() const
+	{
+		return b3TypedConstraintType(m_objectType);
+	}
+
+	void setDbgDrawSize(b3Scalar dbgDrawSize)
+	{
+		m_dbgDrawSize = dbgDrawSize;
+	}
+	b3Scalar getDbgDrawSize()
+	{
+		return m_dbgDrawSize;
+	}
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
+
+	///return the local value of parameter
+	virtual b3Scalar getParam(int num, int axis = -1) const = 0;
+
+	//	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	//virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
+};
+
+// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
+// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
+B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
+{
+	if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	{
+		return angleInRadians;
+	}
+	else if (angleInRadians < angleLowerLimitInRadians)
+	{
+		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
+		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
+	}
+	else if (angleInRadians > angleUpperLimitInRadians)
+	{
+		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+		return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+// clang-format off
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	b3TypedConstraintData
+{
+	int		m_bodyA;
+	int		m_bodyB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	float	m_appliedImpulse;
+	float	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	int	m_overrideNumSolverIterations;
+
+	float	m_breakingImpulseThreshold;
+	int		m_isEnabled;
+	
+};
+
+// clang-format on
+
+/*B3_FORCE_INLINE	int	b3TypedConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(b3TypedConstraintData);
+}
+*/
+
+class b3AngularLimit
+{
+private:
+	b3Scalar
+		m_center,
+		m_halfRange,
+		m_softness,
+		m_biasFactor,
+		m_relaxationFactor,
+		m_correction,
+		m_sign;
+
+	bool
+		m_solveLimit;
+
+public:
+	/// Default constructor initializes limit as inactive, allowing free constraint movement
+	b3AngularLimit()
+		: m_center(0.0f),
+		  m_halfRange(-1.0f),
+		  m_softness(0.9f),
+		  m_biasFactor(0.3f),
+		  m_relaxationFactor(1.0f),
+		  m_correction(0.0f),
+		  m_sign(0.0f),
+		  m_solveLimit(false)
+	{
+	}
+
+	/// Sets all limit's parameters.
+	/// When low > high limit becomes inactive.
+	/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
+	void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f);
+
+	/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
+	/// correction is calculated.
+	void test(const b3Scalar angle);
+
+	/// Returns limit's softness
+	inline b3Scalar getSoftness() const
+	{
+		return m_softness;
+	}
+
+	/// Returns limit's bias factor
+	inline b3Scalar getBiasFactor() const
+	{
+		return m_biasFactor;
+	}
+
+	/// Returns limit's relaxation factor
+	inline b3Scalar getRelaxationFactor() const
+	{
+		return m_relaxationFactor;
+	}
+
+	/// Returns correction value evaluated when test() was invoked
+	inline b3Scalar getCorrection() const
+	{
+		return m_correction;
+	}
+
+	/// Returns sign value evaluated when test() was invoked
+	inline b3Scalar getSign() const
+	{
+		return m_sign;
+	}
+
+	/// Gives half of the distance between min and max limit angle
+	inline b3Scalar getHalfRange() const
+	{
+		return m_halfRange;
+	}
+
+	/// Returns true when the last test() invocation recognized limit violation
+	inline bool isLimit() const
+	{
+		return m_solveLimit;
+	}
+
+	/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
+	/// returned is modified so it equals to the limit closest to given angle.
+	void fit(b3Scalar& angle) const;
+
+	/// Returns correction value multiplied by sign value
+	b3Scalar getError() const;
+
+	b3Scalar getLow() const;
+
+	b3Scalar getHigh() const;
+};
+
+#endif  //B3_TYPED_CONSTRAINT_H

Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp

@@ -0,0 +1,447 @@
+#include "b3CpuRigidBodyPipeline.h"
+
+#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
+#include "Bullet3Dynamics/shared/b3Inertia.h"
+
+struct b3CpuRigidBodyPipelineInternalData
+{
+	b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
+	b3AlignedObjectArray<b3Inertia> m_inertias;
+	b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace;
+
+	b3DynamicBvhBroadphase* m_bp;
+	b3CpuNarrowPhase* m_np;
+	b3Config m_config;
+};
+
+b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
+{
+	m_data = new b3CpuRigidBodyPipelineInternalData;
+	m_data->m_np = narrowphase;
+	m_data->m_bp = broadphaseDbvt;
+	m_data->m_config = config;
+}
+
+b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
+{
+	delete m_data;
+}
+
+void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
+{
+	for (int i = 0; i < this->getNumBodies(); i++)
+	{
+		b3RigidBodyData* body = &m_data->m_rigidBodies[i];
+		b3Float4 position = body->m_pos;
+		b3Quat orientation = body->m_quat;
+
+		int collidableIndex = body->m_collidableIdx;
+		b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
+		int shapeIndex = collidable.m_shapeIndex;
+
+		if (shapeIndex >= 0)
+		{
+			b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
+			b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
+			float margin = 0.f;
+			b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
+			m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
+		}
+	}
+}
+
+void b3CpuRigidBodyPipeline::computeOverlappingPairs()
+{
+	int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
+	m_data->m_bp->calculateOverlappingPairs();
+	numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
+	printf("numPairs=%d\n", numPairs);
+}
+
+void b3CpuRigidBodyPipeline::computeContactPoints()
+{
+	b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
+
+	m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
+}
+void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
+{
+	//update world space aabb's
+	updateAabbWorldSpace();
+
+	//compute overlapping pairs
+	computeOverlappingPairs();
+
+	//compute contacts
+	computeContactPoints();
+
+	//solve contacts
+
+	//update transforms
+	integrate(deltaTime);
+}
+
+static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
+								 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
+{
+	return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
+}
+
+static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
+										 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
+{
+	linear = -n;
+	angular0 = -b3Cross(r0, n);
+	angular1 = b3Cross(r1, n);
+}
+
+static inline void b3SolveContact(b3ContactConstraint4& cs,
+								  const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								  const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								  float maxRambdaDt[4], float minRambdaDt[4])
+{
+	b3Vector3 dLinVelA;
+	dLinVelA.setZero();
+	b3Vector3 dAngVelA;
+	dAngVelA.setZero();
+	b3Vector3 dLinVelB;
+	dLinVelB.setZero();
+	b3Vector3 dAngVelB;
+	dAngVelB.setZero();
+
+	for (int ic = 0; ic < 4; ic++)
+	{
+		//	dont necessary because this makes change to 0
+		if (cs.m_jacCoeffInv[ic] == 0.f) continue;
+
+		{
+			b3Vector3 angular0, angular1, linear;
+			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
+			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
+			b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
+
+			float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
+										  linVelA, angVelA, linVelB, angVelB) +
+							 cs.m_b[ic];
+			rambdaDt *= cs.m_jacCoeffInv[ic];
+
+			{
+				float prevSum = cs.m_appliedRambdaDt[ic];
+				float updated = prevSum;
+				updated += rambdaDt;
+				updated = b3Max(updated, minRambdaDt[ic]);
+				updated = b3Min(updated, maxRambdaDt[ic]);
+				rambdaDt = updated - prevSum;
+				cs.m_appliedRambdaDt[ic] = updated;
+			}
+
+			b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+			b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
+#ifdef _WIN32
+			b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp1.getX()));
+#endif
+			{
+				linVelA += linImp0;
+				angVelA += angImp0;
+				linVelB += linImp1;
+				angVelB += angImp1;
+			}
+		}
+	}
+}
+
+static inline void b3SolveFriction(b3ContactConstraint4& cs,
+								   const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								   const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								   float maxRambdaDt[4], float minRambdaDt[4])
+{
+	if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
+	const b3Vector3& center = (const b3Vector3&)cs.m_center;
+
+	b3Vector3 n = -(const b3Vector3&)cs.m_linear;
+
+	b3Vector3 tangent[2];
+
+	b3PlaneSpace1(n, tangent[0], tangent[1]);
+
+	b3Vector3 angular0, angular1, linear;
+	b3Vector3 r0 = center - posA;
+	b3Vector3 r1 = center - posB;
+	for (int i = 0; i < 2; i++)
+	{
+		b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
+		float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
+									  linVelA, angVelA, linVelB, angVelB);
+		rambdaDt *= cs.m_fJacCoeffInv[i];
+
+		{
+			float prevSum = cs.m_fAppliedRambdaDt[i];
+			float updated = prevSum;
+			updated += rambdaDt;
+			updated = b3Max(updated, minRambdaDt[i]);
+			updated = b3Min(updated, maxRambdaDt[i]);
+			rambdaDt = updated - prevSum;
+			cs.m_fAppliedRambdaDt[i] = updated;
+		}
+
+		b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+		b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+		b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+		b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
+#ifdef _WIN32
+		b3Assert(_finite(linImp0.getX()));
+		b3Assert(_finite(linImp1.getX()));
+#endif
+		linVelA += linImp0;
+		angVelA += angImp0;
+		linVelB += linImp1;
+		angVelB += angImp1;
+	}
+
+	{  //	angular damping for point constraint
+		b3Vector3 ab = (posB - posA).normalized();
+		b3Vector3 ac = (center - posA).normalized();
+		if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
+		{
+			float angNA = b3Dot(n, angVelA);
+			float angNB = b3Dot(n, angVelB);
+
+			angVelA -= (angNA * 0.1f) * n;
+			angVelB -= (angNB * 0.1f) * n;
+		}
+	}
+}
+
+struct b3SolveTask  // : public ThreadPool::Task
+{
+	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
+				b3AlignedObjectArray<b3Inertia>& shapes,
+				b3AlignedObjectArray<b3ContactConstraint4>& constraints,
+				int start, int nConstraints,
+				int maxNumBatches,
+				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
+		: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
+	{
+	}
+
+	unsigned short int getType() { return 0; }
+
+	void run(int tIdx)
+	{
+		b3AlignedObjectArray<int> usedBodies;
+		//printf("run..............\n");
+
+		for (int bb = 0; bb < m_maxNumBatches; bb++)
+		{
+			usedBodies.resize(0);
+			for (int ic = m_nConstraints - 1; ic >= 0; ic--)
+			//for(int ic=0; ic<m_nConstraints; ic++)
+			{
+				int i = m_start + ic;
+				if (m_constraints[i].m_batchIdx != bb)
+					continue;
+
+				float frictionCoeff = b3GetFrictionCoeff(&m_constraints[i]);
+				int aIdx = (int)m_constraints[i].m_bodyA;
+				int bIdx = (int)m_constraints[i].m_bodyB;
+				//int localBatch = m_constraints[i].m_batchIdx;
+				b3RigidBodyData& bodyA = m_bodies[aIdx];
+				b3RigidBodyData& bodyB = m_bodies[bIdx];
+
+#if 0
+				if ((bodyA.m_invMass) && (bodyB.m_invMass))
+				{
+				//	printf("aIdx=%d, bIdx=%d\n", aIdx,bIdx);
+				}
+				if (bIdx==10)
+				{
+					//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
+				}
+#endif
+				if (aIdx == 10)
+				{
+					//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
+				}
+				if (usedBodies.size() < (aIdx + 1))
+				{
+					usedBodies.resize(aIdx + 1, 0);
+				}
+
+				if (usedBodies.size() < (bIdx + 1))
+				{
+					usedBodies.resize(bIdx + 1, 0);
+				}
+
+				if (bodyA.m_invMass)
+				{
+					b3Assert(usedBodies[aIdx] == 0);
+					usedBodies[aIdx]++;
+				}
+
+				if (bodyB.m_invMass)
+				{
+					b3Assert(usedBodies[bIdx] == 0);
+					usedBodies[bIdx]++;
+				}
+
+				if (!m_solveFriction)
+				{
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
+
+					b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+								   (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+								   maxRambdaDt, minRambdaDt);
+				}
+				else
+				{
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
+
+					float sum = 0;
+					for (int j = 0; j < 4; j++)
+					{
+						sum += m_constraints[i].m_appliedRambdaDt[j];
+					}
+					frictionCoeff = 0.7f;
+					for (int j = 0; j < 4; j++)
+					{
+						maxRambdaDt[j] = frictionCoeff * sum;
+						minRambdaDt[j] = -maxRambdaDt[j];
+					}
+
+					b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+									(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+									maxRambdaDt, minRambdaDt);
+				}
+			}
+
+			if (m_wgUsedBodies)
+			{
+				if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
+				{
+					m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
+				}
+				for (int i = 0; i < usedBodies.size(); i++)
+				{
+					if (usedBodies[i])
+					{
+						//printf("cell %d uses body %d\n", m_curWgidx,i);
+						m_wgUsedBodies[m_curWgidx][i] = 1;
+					}
+				}
+			}
+		}
+	}
+
+	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
+	b3AlignedObjectArray<b3Inertia>& m_shapes;
+	b3AlignedObjectArray<b3ContactConstraint4>& m_constraints;
+	b3AlignedObjectArray<int>* m_wgUsedBodies;
+	int m_curWgidx;
+	int m_start;
+	int m_nConstraints;
+	bool m_solveFriction;
+	int m_maxNumBatches;
+};
+
+void b3CpuRigidBodyPipeline::solveContactConstraints()
+{
+	int m_nIterations = 4;
+
+	b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
+	//	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
+	int n = contactConstraints.size();
+	//convert contacts...
+
+	int maxNumBatches = 250;
+
+	for (int iter = 0; iter < m_nIterations; iter++)
+	{
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
+		task.m_solveFriction = false;
+		task.run(0);
+	}
+
+	for (int iter = 0; iter < m_nIterations; iter++)
+	{
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
+		task.m_solveFriction = true;
+		task.run(0);
+	}
+}
+
+void b3CpuRigidBodyPipeline::integrate(float deltaTime)
+{
+	float angDamping = 0.f;
+	b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
+
+	//integrate transforms (external forces/gravity should be moved into constraint solver)
+	for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
+	{
+		b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
+	}
+}
+
+int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
+{
+	b3RigidBodyData body;
+	int bodyIndex = m_data->m_rigidBodies.size();
+	body.m_invMass = mass ? 1.f / mass : 0.f;
+	body.m_angVel.setValue(0, 0, 0);
+	body.m_collidableIdx = collidableIndex;
+	body.m_frictionCoeff = 0.3f;
+	body.m_linVel.setValue(0, 0, 0);
+	body.m_pos.setValue(position[0], position[1], position[2]);
+	body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
+	body.m_restituitionCoeff = 0.f;
+
+	m_data->m_rigidBodies.push_back(body);
+
+	if (collidableIndex >= 0)
+	{
+		b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
+
+		b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
+		b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
+		b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
+
+		b3Scalar margin = 0.01f;
+		b3Transform t;
+		t.setIdentity();
+		t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
+		t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
+		b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
+
+		m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
+		//		b3Vector3 aabbMin,aabbMax;
+		//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+	}
+	else
+	{
+		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
+	}
+
+	return bodyIndex;
+}
+
+const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
+{
+	return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
+}
+
+int b3CpuRigidBodyPipeline::getNumBodies() const
+{
+	return m_data->m_rigidBodies.size();
+}

Engine/lib/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h

@@ -0,0 +1,62 @@
+/*
+Copyright (c) 2013 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
+#define B3_CPU_RIGIDBODY_PIPELINE_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
+
+class b3CpuRigidBodyPipeline
+{
+protected:
+	struct b3CpuRigidBodyPipelineInternalData* m_data;
+
+	int allocateCollidable();
+
+public:
+	b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
+	virtual ~b3CpuRigidBodyPipeline();
+
+	virtual void stepSimulation(float deltaTime);
+	virtual void integrate(float timeStep);
+	virtual void updateAabbWorldSpace();
+	virtual void computeOverlappingPairs();
+	virtual void computeContactPoints();
+	virtual void solveContactConstraints();
+
+	int registerConvexPolyhedron(class b3ConvexUtility* convex);
+
+	int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
+	void writeAllInstancesToGpu();
+	void copyConstraintsToHost();
+	void setGravity(const float* grav);
+	void reset();
+
+	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
+	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
+	void removeConstraintByUid(int uid);
+
+	void addConstraint(class b3TypedConstraint* constraint);
+	void removeConstraint(b3TypedConstraint* constraint);
+
+	void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
+
+	const struct b3RigidBodyData* getBodyBuffer() const;
+
+	int getNumBodies() const;
+};
+
+#endif  //B3_CPU_RIGIDBODY_PIPELINE_H

Engine/lib/bullet/src/Bullet3Dynamics/premake4.lua

@@ -0,0 +1,18 @@
+	project "Bullet3Dynamics"
+
+	language "C++"
+				
+	kind "StaticLib"
+
+	includedirs {
+		".."
+	}		
+	
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h

@@ -0,0 +1,31 @@
+#ifndef B3_CONTACT_CONSTRAINT5_H
+#define B3_CONTACT_CONSTRAINT5_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
+
+struct b3ContactConstraint4
+{
+	b3Float4 m_linear;  //normal?
+	b3Float4 m_worldPos[4];
+	b3Float4 m_center;  //	friction
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+	float m_fJacCoeffInv[2];      //	friction
+	float m_fAppliedRambdaDt[2];  //	friction
+
+	unsigned int m_bodyA;
+	unsigned int m_bodyB;
+	int m_batchIdx;
+	unsigned int m_paddings;
+};
+
+//inline	void setFrictionCoeff(float value) { m_linear[3] = value; }
+inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
+{
+	return constraint->m_linear.w;
+}
+
+#endif  //B3_CONTACT_CONSTRAINT5_H

Engine/lib/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h

@@ -0,0 +1,148 @@
+
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
+{
+	if (b3Fabs(n.z) > 0.70710678f)
+	{
+		// choose p in y-z plane
+		float a = n.y * n.y + n.z * n.z;
+		float k = 1.f / sqrt(a);
+		p[0].x = 0;
+		p[0].y = -n.z * k;
+		p[0].z = n.y * k;
+		// set q = n x p
+		q[0].x = a * k;
+		q[0].y = -n.x * p[0].z;
+		q[0].z = n.x * p[0].y;
+	}
+	else
+	{
+		// choose p in x-y plane
+		float a = n.x * n.x + n.y * n.y;
+		float k = 1.f / sqrt(a);
+		p[0].x = -n.y * k;
+		p[0].y = n.x * k;
+		p[0].z = 0;
+		// set q = n x p
+		q[0].x = -n.z * p[0].y;
+		q[0].y = n.z * p[0].x;
+		q[0].z = a * k;
+	}
+}
+
+void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
+{
+	*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
+	*angular0 = b3Cross3(r0, n);
+	*angular1 = -b3Cross3(r1, n);
+}
+
+float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
+				 b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
+{
+	return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
+}
+
+float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
+				   float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
+{
+	//	linear0,1 are normlized
+	float jmj0 = invMass0;  //b3Dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
+	float jmj2 = invMass1;  //b3Dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
+	return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
+}
+
+void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
+					b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
+					__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
+					b3ContactConstraint4_t* dstC)
+{
+	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
+	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
+
+	float dtInv = 1.f / dt;
+	for (int ic = 0; ic < 4; ic++)
+	{
+		dstC->m_appliedRambdaDt[ic] = 0.f;
+	}
+	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
+
+	dstC->m_linear = src->m_worldNormalOnB;
+	dstC->m_linear.w = 0.7f;  //src->getFrictionCoeff() );
+	for (int ic = 0; ic < 4; ic++)
+	{
+		b3Float4 r0 = src->m_worldPosB[ic] - posA;
+		b3Float4 r1 = src->m_worldPosB[ic] - posB;
+
+		if (ic >= src->m_worldNormalOnB.w)  //npoints
+		{
+			dstC->m_jacCoeffInv[ic] = 0.f;
+			continue;
+		}
+
+		float relVelN;
+		{
+			b3Float4 linear, angular0, angular1;
+			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
+
+			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
+
+			relVelN = calcRelVel(linear, -linear, angular0, angular1,
+								 linVelA, angVelA, linVelB, angVelB);
+
+			float e = 0.f;  //src->getRestituitionCoeff();
+			if (relVelN * relVelN < 0.004f) e = 0.f;
+
+			dstC->m_b[ic] = e * relVelN;
+			//float penetration = src->m_worldPosB[ic].w;
+			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
+			dstC->m_appliedRambdaDt[ic] = 0.f;
+		}
+	}
+
+	if (src->m_worldNormalOnB.w > 0)  //npoints
+	{                                 //	prepare friction
+		b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+		for (int i = 0; i < src->m_worldNormalOnB.w; i++)
+			center += src->m_worldPosB[i];
+		center /= (float)src->m_worldNormalOnB.w;
+
+		b3Float4 tangent[2];
+		b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
+
+		b3Float4 r[2];
+		r[0] = center - posA;
+		r[1] = center - posB;
+
+		for (int i = 0; i < 2; i++)
+		{
+			b3Float4 linear, angular0, angular1;
+			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
+
+			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
+			dstC->m_fAppliedRambdaDt[i] = 0.f;
+		}
+		dstC->m_center = center;
+	}
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (i < src->m_worldNormalOnB.w)
+		{
+			dstC->m_worldPos[i] = src->m_worldPosB[i];
+		}
+		else
+		{
+			dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+		}
+	}
+}

Engine/lib/bullet/src/Bullet3Dynamics/shared/b3Inertia.h

@@ -0,0 +1,14 @@
+
+
+#ifndef B3_INERTIA_H
+#define B3_INERTIA_H
+
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+struct b3Inertia
+{
+	b3Mat3x3 m_invInertiaWorld;
+	b3Mat3x3 m_initInvInertia;
+};
+
+#endif  //B3_INERTIA_H

Engine/lib/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h

@@ -0,0 +1,106 @@
+
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+{
+	if (bodies[nodeID].m_invMass != 0.f)
+	{
+		float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
+
+		//angular velocity
+		{
+			b3Float4 axis;
+			//add some hardcoded angular damping
+			bodies[nodeID].m_angVel.x *= angularDamping;
+			bodies[nodeID].m_angVel.y *= angularDamping;
+			bodies[nodeID].m_angVel.z *= angularDamping;
+
+			b3Float4 angvel = bodies[nodeID].m_angVel;
+
+			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
+
+			//limit the angular motion
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
+			}
+			if (fAngle < 0.001f)
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
+			}
+
+			b3Quat dorn;
+			dorn.x = axis.x;
+			dorn.y = axis.y;
+			dorn.z = axis.z;
+			dorn.w = b3Cos(fAngle * timeStep * 0.5f);
+			b3Quat orn0 = bodies[nodeID].m_quat;
+			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
+			predictedOrn = b3QuatNormalized(predictedOrn);
+			bodies[nodeID].m_quat = predictedOrn;
+		}
+		//linear velocity
+		bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
+
+		//apply gravity
+		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
+	}
+}
+
+inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+{
+	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
+
+	if ((body->m_invMass != 0.f))
+	{
+		//angular velocity
+		{
+			b3Float4 axis;
+			//add some hardcoded angular damping
+			body->m_angVel.x *= angularDamping;
+			body->m_angVel.y *= angularDamping;
+			body->m_angVel.z *= angularDamping;
+
+			b3Float4 angvel = body->m_angVel;
+			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
+			//limit the angular motion
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
+			}
+			if (fAngle < 0.001f)
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
+			}
+			b3Quat dorn;
+			dorn.x = axis.x;
+			dorn.y = axis.y;
+			dorn.z = axis.z;
+			dorn.w = b3Cos(fAngle * timeStep * 0.5f);
+			b3Quat orn0 = body->m_quat;
+
+			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
+			predictedOrn = b3QuatNormalized(predictedOrn);
+			body->m_quat = predictedOrn;
+		}
+
+		//apply gravity
+		body->m_linVel += gravityAcceleration * timeStep;
+
+		//linear velocity
+		body->m_pos += body->m_linVel * timeStep;
+	}
+}

Engine/lib/bullet/src/Bullet3Geometry/CMakeLists.txt

@@ -0,0 +1,47 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Geometry_SRCS
+	b3ConvexHullComputer.cpp
+	b3GeometryUtil.cpp
+)
+
+SET(Bullet3Geometry_HDRS
+	b3AabbUtil.h
+	b3ConvexHullComputer.h
+	b3GeometryUtil.h
+	b3GrahamScan2dConvexHull.h
+)
+
+ADD_LIBRARY(Bullet3Geometry ${Bullet3Geometry_SRCS} ${Bullet3Geometry_HDRS})
+if (BUILD_SHARED_LIBS)
+  target_link_libraries(Bullet3Geometry Bullet3Common)
+endif()
+SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Geometry DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Geometry
+					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)
+			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(Bullet3Geometry PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES PUBLIC_HEADER "${Bullet3Geometry_HDRS}")
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

Engine/lib/bullet/src/Bullet3Geometry/b3AabbUtil.h

@@ -0,0 +1,217 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_AABB_UTIL2
+#define B3_AABB_UTIL2
+
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3MinMax.h"
+
+B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
+								  b3Vector3& aabbMax,
+								  const b3Vector3& expansionMin,
+								  const b3Vector3& expansionMax)
+{
+	aabbMin = aabbMin + expansionMin;
+	aabbMax = aabbMax + expansionMax;
+}
+
+/// conservative test for overlap between two aabbs
+B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
+											 const b3Vector3& point)
+{
+	bool overlap = true;
+	overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
+	overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
+	overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between two aabbs
+B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
+											const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
+	overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
+	overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
+												const b3Vector3& aabbMin, const b3Vector3& aabbMax)
+{
+	const b3Vector3& p1 = vertices[0];
+	const b3Vector3& p2 = vertices[1];
+	const b3Vector3& p3 = vertices[2];
+
+	if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+	if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+	if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+	if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+
+	if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+	if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+	return true;
+}
+
+B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& halfExtent)
+{
+	return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
+		   (p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
+		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
+		   (p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
+		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
+		   (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
+								const b3Vector3& rayInvDirection,
+								const unsigned int raySign[3],
+								const b3Vector3 bounds[2],
+								b3Scalar& tmin,
+								b3Scalar lambda_min,
+								b3Scalar lambda_max)
+{
+	b3Scalar tmax, tymin, tymax, tzmin, tzmax;
+	tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+	tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+	tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+	tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+
+	if ((tmin > tymax) || (tymin > tmax))
+		return false;
+
+	if (tymin > tmin)
+		tmin = tymin;
+
+	if (tymax < tmax)
+		tmax = tymax;
+
+	tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+	tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+
+	if ((tmin > tzmax) || (tzmin > tmax))
+		return false;
+	if (tzmin > tmin)
+		tmin = tzmin;
+	if (tzmax < tmax)
+		tmax = tzmax;
+	return ((tmin < lambda_max) && (tmax > lambda_min));
+}
+
+B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
+							   const b3Vector3& rayTo,
+							   const b3Vector3& aabbMin,
+							   const b3Vector3& aabbMax,
+							   b3Scalar& param, b3Vector3& normal)
+{
+	b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
+	b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
+	b3Vector3 source = rayFrom - aabbCenter;
+	b3Vector3 target = rayTo - aabbCenter;
+	int sourceOutcode = b3Outcode(source, aabbHalfExtent);
+	int targetOutcode = b3Outcode(target, aabbHalfExtent);
+	if ((sourceOutcode & targetOutcode) == 0x0)
+	{
+		b3Scalar lambda_enter = b3Scalar(0.0);
+		b3Scalar lambda_exit = param;
+		b3Vector3 r = target - source;
+		int i;
+		b3Scalar normSign = 1;
+		b3Vector3 hitNormal = b3MakeVector3(0, 0, 0);
+		int bit = 1;
+
+		for (int j = 0; j < 2; j++)
+		{
+			for (i = 0; i != 3; ++i)
+			{
+				if (sourceOutcode & bit)
+				{
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
+					if (lambda_enter <= lambda)
+					{
+						lambda_enter = lambda;
+						hitNormal.setValue(0, 0, 0);
+						hitNormal[i] = normSign;
+					}
+				}
+				else if (targetOutcode & bit)
+				{
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
+					b3SetMin(lambda_exit, lambda);
+				}
+				bit <<= 1;
+			}
+			normSign = b3Scalar(-1.);
+		}
+		if (lambda_enter <= lambda_exit)
+		{
+			param = lambda_enter;
+			normal = hitNormal;
+			return true;
+		}
+	}
+	return false;
+}
+
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
+{
+	b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
+	b3Matrix3x3 abs_b = t.getBasis().absolute();
+	b3Vector3 center = t.getOrigin();
+	b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
+}
+
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
+{
+	//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
+	//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
+	//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
+	b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
+	localHalfExtents += b3MakeVector3(margin, margin, margin);
+
+	b3Vector3 localCenter = b3Scalar(0.5) * (localAabbMax + localAabbMin);
+	b3Matrix3x3 abs_b = trans.getBasis().absolute();
+	b3Vector3 center = trans(localCenter);
+	b3Vector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
+}
+
+#define B3_USE_BANCHLESS 1
+#ifdef B3_USE_BANCHLESS
+//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+	return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+											  1, 0));
+}
+#else
+B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+#endif  //B3_USE_BANCHLESS
+
+#endif  //B3_AABB_UTIL2

Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp

@@ -0,0 +1,2745 @@
+/*
+Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <string.h>
+
+#include "b3ConvexHullComputer.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3MinMax.h"
+#include "Bullet3Common/b3Vector3.h"
+
+#ifdef __GNUC__
+#include <stdint.h>
+typedef int32_t btInt32_t;
+typedef int64_t btInt64_t;
+typedef uint32_t btUint32_t;
+typedef uint64_t btUint64_t;
+#elif defined(_MSC_VER)
+typedef __int32 btInt32_t;
+typedef __int64 btInt64_t;
+typedef unsigned __int32 btUint32_t;
+typedef unsigned __int64 btUint64_t;
+#else
+typedef int btInt32_t;
+typedef long long int btInt64_t;
+typedef unsigned int btUint32_t;
+typedef unsigned long long int btUint64_t;
+#endif
+
+//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
+//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL))  // || (defined(__ICL) && defined(_M_X64))   bug in Intel compiler, disable inline assembly
+//	#define USE_X86_64_ASM
+//#endif
+
+//#define DEBUG_CONVEX_HULL
+//#define SHOW_ITERATIONS
+
+#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
+#include <stdio.h>
+#endif
+
+// Convex hull implementation based on Preparata and Hong
+// Ole Kniemeyer, MAXON Computer GmbH
+class b3ConvexHullInternal
+{
+public:
+	class Point64
+	{
+	public:
+		btInt64_t x;
+		btInt64_t y;
+		btInt64_t z;
+
+		Point64(btInt64_t x, btInt64_t y, btInt64_t z) : x(x), y(y), z(z)
+		{
+		}
+
+		bool isZero()
+		{
+			return (x == 0) && (y == 0) && (z == 0);
+		}
+
+		btInt64_t dot(const Point64& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
+	};
+
+	class Point32
+	{
+	public:
+		btInt32_t x;
+		btInt32_t y;
+		btInt32_t z;
+		int index;
+
+		Point32()
+		{
+		}
+
+		Point32(btInt32_t x, btInt32_t y, btInt32_t z) : x(x), y(y), z(z), index(-1)
+		{
+		}
+
+		bool operator==(const Point32& b) const
+		{
+			return (x == b.x) && (y == b.y) && (z == b.z);
+		}
+
+		bool operator!=(const Point32& b) const
+		{
+			return (x != b.x) || (y != b.y) || (z != b.z);
+		}
+
+		bool isZero()
+		{
+			return (x == 0) && (y == 0) && (z == 0);
+		}
+
+		Point64 cross(const Point32& b) const
+		{
+			return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+		}
+
+		Point64 cross(const Point64& b) const
+		{
+			return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+		}
+
+		btInt64_t dot(const Point32& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
+
+		btInt64_t dot(const Point64& b) const
+		{
+			return x * b.x + y * b.y + z * b.z;
+		}
+
+		Point32 operator+(const Point32& b) const
+		{
+			return Point32(x + b.x, y + b.y, z + b.z);
+		}
+
+		Point32 operator-(const Point32& b) const
+		{
+			return Point32(x - b.x, y - b.y, z - b.z);
+		}
+	};
+
+	class Int128
+	{
+	public:
+		btUint64_t low;
+		btUint64_t high;
+
+		Int128()
+		{
+		}
+
+		Int128(btUint64_t low, btUint64_t high) : low(low), high(high)
+		{
+		}
+
+		Int128(btUint64_t low) : low(low), high(0)
+		{
+		}
+
+		Int128(btInt64_t value) : low(value), high((value >= 0) ? 0 : (btUint64_t)-1LL)
+		{
+		}
+
+		static Int128 mul(btInt64_t a, btInt64_t b);
+
+		static Int128 mul(btUint64_t a, btUint64_t b);
+
+		Int128 operator-() const
+		{
+			return Int128((btUint64_t) - (btInt64_t)low, ~high + (low == 0));
+		}
+
+		Int128 operator+(const Int128& b) const
+		{
+#ifdef USE_X86_64_ASM
+			Int128 result;
+			__asm__(
+				"addq %[bl], %[rl]\n\t"
+				"adcq %[bh], %[rh]\n\t"
+				: [rl] "=r"(result.low), [rh] "=r"(result.high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
+			return result;
+#else
+			btUint64_t lo = low + b.low;
+			return Int128(lo, high + b.high + (lo < low));
+#endif
+		}
+
+		Int128 operator-(const Int128& b) const
+		{
+#ifdef USE_X86_64_ASM
+			Int128 result;
+			__asm__(
+				"subq %[bl], %[rl]\n\t"
+				"sbbq %[bh], %[rh]\n\t"
+				: [rl] "=r"(result.low), [rh] "=r"(result.high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
+			return result;
+#else
+			return *this + -b;
+#endif
+		}
+
+		Int128& operator+=(const Int128& b)
+		{
+#ifdef USE_X86_64_ASM
+			__asm__(
+				"addq %[bl], %[rl]\n\t"
+				"adcq %[bh], %[rh]\n\t"
+				: [rl] "=r"(low), [rh] "=r"(high)
+				: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+				: "cc");
+#else
+			btUint64_t lo = low + b.low;
+			if (lo < low)
+			{
+				++high;
+			}
+			low = lo;
+			high += b.high;
+#endif
+			return *this;
+		}
+
+		Int128& operator++()
+		{
+			if (++low == 0)
+			{
+				++high;
+			}
+			return *this;
+		}
+
+		Int128 operator*(btInt64_t b) const;
+
+		b3Scalar toScalar() const
+		{
+			return ((btInt64_t)high >= 0) ? b3Scalar(high) * (b3Scalar(0x100000000LL) * b3Scalar(0x100000000LL)) + b3Scalar(low)
+										  : -(-*this).toScalar();
+		}
+
+		int getSign() const
+		{
+			return ((btInt64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
+		}
+
+		bool operator<(const Int128& b) const
+		{
+			return (high < b.high) || ((high == b.high) && (low < b.low));
+		}
+
+		int ucmp(const Int128& b) const
+		{
+			if (high < b.high)
+			{
+				return -1;
+			}
+			if (high > b.high)
+			{
+				return 1;
+			}
+			if (low < b.low)
+			{
+				return -1;
+			}
+			if (low > b.low)
+			{
+				return 1;
+			}
+			return 0;
+		}
+	};
+
+	class Rational64
+	{
+	private:
+		btUint64_t m_numerator;
+		btUint64_t m_denominator;
+		int sign;
+
+	public:
+		Rational64(btInt64_t numerator, btInt64_t denominator)
+		{
+			if (numerator > 0)
+			{
+				sign = 1;
+				m_numerator = (btUint64_t)numerator;
+			}
+			else if (numerator < 0)
+			{
+				sign = -1;
+				m_numerator = (btUint64_t)-numerator;
+			}
+			else
+			{
+				sign = 0;
+				m_numerator = 0;
+			}
+			if (denominator > 0)
+			{
+				m_denominator = (btUint64_t)denominator;
+			}
+			else if (denominator < 0)
+			{
+				sign = -sign;
+				m_denominator = (btUint64_t)-denominator;
+			}
+			else
+			{
+				m_denominator = 0;
+			}
+		}
+
+		bool isNegativeInfinity() const
+		{
+			return (sign < 0) && (m_denominator == 0);
+		}
+
+		bool isNaN() const
+		{
+			return (sign == 0) && (m_denominator == 0);
+		}
+
+		int compare(const Rational64& b) const;
+
+		b3Scalar toScalar() const
+		{
+			return sign * ((m_denominator == 0) ? B3_INFINITY : (b3Scalar)m_numerator / m_denominator);
+		}
+	};
+
+	class Rational128
+	{
+	private:
+		Int128 numerator;
+		Int128 denominator;
+		int sign;
+		bool isInt64;
+
+	public:
+		Rational128(btInt64_t value)
+		{
+			if (value > 0)
+			{
+				sign = 1;
+				this->numerator = value;
+			}
+			else if (value < 0)
+			{
+				sign = -1;
+				this->numerator = -value;
+			}
+			else
+			{
+				sign = 0;
+				this->numerator = (btUint64_t)0;
+			}
+			this->denominator = (btUint64_t)1;
+			isInt64 = true;
+		}
+
+		Rational128(const Int128& numerator, const Int128& denominator)
+		{
+			sign = numerator.getSign();
+			if (sign >= 0)
+			{
+				this->numerator = numerator;
+			}
+			else
+			{
+				this->numerator = -numerator;
+			}
+			int dsign = denominator.getSign();
+			if (dsign >= 0)
+			{
+				this->denominator = denominator;
+			}
+			else
+			{
+				sign = -sign;
+				this->denominator = -denominator;
+			}
+			isInt64 = false;
+		}
+
+		int compare(const Rational128& b) const;
+
+		int compare(btInt64_t b) const;
+
+		b3Scalar toScalar() const
+		{
+			return sign * ((denominator.getSign() == 0) ? B3_INFINITY : numerator.toScalar() / denominator.toScalar());
+		}
+	};
+
+	class PointR128
+	{
+	public:
+		Int128 x;
+		Int128 y;
+		Int128 z;
+		Int128 denominator;
+
+		PointR128()
+		{
+		}
+
+		PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator) : x(x), y(y), z(z), denominator(denominator)
+		{
+		}
+
+		b3Scalar xvalue() const
+		{
+			return x.toScalar() / denominator.toScalar();
+		}
+
+		b3Scalar yvalue() const
+		{
+			return y.toScalar() / denominator.toScalar();
+		}
+
+		b3Scalar zvalue() const
+		{
+			return z.toScalar() / denominator.toScalar();
+		}
+	};
+
+	class Edge;
+	class Face;
+
+	class Vertex
+	{
+	public:
+		Vertex* next;
+		Vertex* prev;
+		Edge* edges;
+		Face* firstNearbyFace;
+		Face* lastNearbyFace;
+		PointR128 point128;
+		Point32 point;
+		int copy;
+
+		Vertex() : next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1)
+		{
+		}
+
+#ifdef DEBUG_CONVEX_HULL
+		void print()
+		{
+			b3Printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
+		}
+
+		void printGraph();
+#endif
+
+		Point32 operator-(const Vertex& b) const
+		{
+			return point - b.point;
+		}
+
+		Rational128 dot(const Point64& b) const
+		{
+			return (point.index >= 0) ? Rational128(point.dot(b))
+									  : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
+		}
+
+		b3Scalar xvalue() const
+		{
+			return (point.index >= 0) ? b3Scalar(point.x) : point128.xvalue();
+		}
+
+		b3Scalar yvalue() const
+		{
+			return (point.index >= 0) ? b3Scalar(point.y) : point128.yvalue();
+		}
+
+		b3Scalar zvalue() const
+		{
+			return (point.index >= 0) ? b3Scalar(point.z) : point128.zvalue();
+		}
+
+		void receiveNearbyFaces(Vertex* src)
+		{
+			if (lastNearbyFace)
+			{
+				lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
+			}
+			else
+			{
+				firstNearbyFace = src->firstNearbyFace;
+			}
+			if (src->lastNearbyFace)
+			{
+				lastNearbyFace = src->lastNearbyFace;
+			}
+			for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
+			{
+				b3Assert(f->nearbyVertex == src);
+				f->nearbyVertex = this;
+			}
+			src->firstNearbyFace = NULL;
+			src->lastNearbyFace = NULL;
+		}
+	};
+
+	class Edge
+	{
+	public:
+		Edge* next;
+		Edge* prev;
+		Edge* reverse;
+		Vertex* target;
+		Face* face;
+		int copy;
+
+		~Edge()
+		{
+			next = NULL;
+			prev = NULL;
+			reverse = NULL;
+			target = NULL;
+			face = NULL;
+		}
+
+		void link(Edge* n)
+		{
+			b3Assert(reverse->target == n->reverse->target);
+			next = n;
+			n->prev = this;
+		}
+
+#ifdef DEBUG_CONVEX_HULL
+		void print()
+		{
+			b3Printf("E%p : %d -> %d,  n=%p p=%p   (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
+					 reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
+		}
+#endif
+	};
+
+	class Face
+	{
+	public:
+		Face* next;
+		Vertex* nearbyVertex;
+		Face* nextWithSameNearbyVertex;
+		Point32 origin;
+		Point32 dir0;
+		Point32 dir1;
+
+		Face() : next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL)
+		{
+		}
+
+		void init(Vertex* a, Vertex* b, Vertex* c)
+		{
+			nearbyVertex = a;
+			origin = a->point;
+			dir0 = *b - *a;
+			dir1 = *c - *a;
+			if (a->lastNearbyFace)
+			{
+				a->lastNearbyFace->nextWithSameNearbyVertex = this;
+			}
+			else
+			{
+				a->firstNearbyFace = this;
+			}
+			a->lastNearbyFace = this;
+		}
+
+		Point64 getNormal()
+		{
+			return dir0.cross(dir1);
+		}
+	};
+
+	template <typename UWord, typename UHWord>
+	class DMul
+	{
+	private:
+		static btUint32_t high(btUint64_t value)
+		{
+			return (btUint32_t)(value >> 32);
+		}
+
+		static btUint32_t low(btUint64_t value)
+		{
+			return (btUint32_t)value;
+		}
+
+		static btUint64_t mul(btUint32_t a, btUint32_t b)
+		{
+			return (btUint64_t)a * (btUint64_t)b;
+		}
+
+		static void shlHalf(btUint64_t& value)
+		{
+			value <<= 32;
+		}
+
+		static btUint64_t high(Int128 value)
+		{
+			return value.high;
+		}
+
+		static btUint64_t low(Int128 value)
+		{
+			return value.low;
+		}
+
+		static Int128 mul(btUint64_t a, btUint64_t b)
+		{
+			return Int128::mul(a, b);
+		}
+
+		static void shlHalf(Int128& value)
+		{
+			value.high = value.low;
+			value.low = 0;
+		}
+
+	public:
+		static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
+		{
+			UWord p00 = mul(low(a), low(b));
+			UWord p01 = mul(low(a), high(b));
+			UWord p10 = mul(high(a), low(b));
+			UWord p11 = mul(high(a), high(b));
+			UWord p0110 = UWord(low(p01)) + UWord(low(p10));
+			p11 += high(p01);
+			p11 += high(p10);
+			p11 += high(p0110);
+			shlHalf(p0110);
+			p00 += p0110;
+			if (p00 < p0110)
+			{
+				++p11;
+			}
+			resLow = p00;
+			resHigh = p11;
+		}
+	};
+
+private:
+	class IntermediateHull
+	{
+	public:
+		Vertex* minXy;
+		Vertex* maxXy;
+		Vertex* minYx;
+		Vertex* maxYx;
+
+		IntermediateHull() : minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL)
+		{
+		}
+
+		void print();
+	};
+
+	enum Orientation
+	{
+		NONE,
+		CLOCKWISE,
+		COUNTER_CLOCKWISE
+	};
+
+	template <typename T>
+	class PoolArray
+	{
+	private:
+		T* array;
+		int size;
+
+	public:
+		PoolArray<T>* next;
+
+		PoolArray(int size) : size(size), next(NULL)
+		{
+			array = (T*)b3AlignedAlloc(sizeof(T) * size, 16);
+		}
+
+		~PoolArray()
+		{
+			b3AlignedFree(array);
+		}
+
+		T* init()
+		{
+			T* o = array;
+			for (int i = 0; i < size; i++, o++)
+			{
+				o->next = (i + 1 < size) ? o + 1 : NULL;
+			}
+			return array;
+		}
+	};
+
+	template <typename T>
+	class Pool
+	{
+	private:
+		PoolArray<T>* arrays;
+		PoolArray<T>* nextArray;
+		T* freeObjects;
+		int arraySize;
+
+	public:
+		Pool() : arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256)
+		{
+		}
+
+		~Pool()
+		{
+			while (arrays)
+			{
+				PoolArray<T>* p = arrays;
+				arrays = p->next;
+				p->~PoolArray<T>();
+				b3AlignedFree(p);
+			}
+		}
+
+		void reset()
+		{
+			nextArray = arrays;
+			freeObjects = NULL;
+		}
+
+		void setArraySize(int arraySize)
+		{
+			this->arraySize = arraySize;
+		}
+
+		T* newObject()
+		{
+			T* o = freeObjects;
+			if (!o)
+			{
+				PoolArray<T>* p = nextArray;
+				if (p)
+				{
+					nextArray = p->next;
+				}
+				else
+				{
+					p = new (b3AlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
+					p->next = arrays;
+					arrays = p;
+				}
+				o = p->init();
+			}
+			freeObjects = o->next;
+			return new (o) T();
+		};
+
+		void freeObject(T* object)
+		{
+			object->~T();
+			object->next = freeObjects;
+			freeObjects = object;
+		}
+	};
+
+	b3Vector3 scaling;
+	b3Vector3 center;
+	Pool<Vertex> vertexPool;
+	Pool<Edge> edgePool;
+	Pool<Face> facePool;
+	b3AlignedObjectArray<Vertex*> originalVertices;
+	int mergeStamp;
+	int minAxis;
+	int medAxis;
+	int maxAxis;
+	int usedEdgePairs;
+	int maxUsedEdgePairs;
+
+	static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
+	Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
+	void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
+
+	Edge* newEdgePair(Vertex* from, Vertex* to);
+
+	void removeEdgePair(Edge* edge)
+	{
+		Edge* n = edge->next;
+		Edge* r = edge->reverse;
+
+		b3Assert(edge->target && r->target);
+
+		if (n != edge)
+		{
+			n->prev = edge->prev;
+			edge->prev->next = n;
+			r->target->edges = n;
+		}
+		else
+		{
+			r->target->edges = NULL;
+		}
+
+		n = r->next;
+
+		if (n != r)
+		{
+			n->prev = r->prev;
+			r->prev->next = n;
+			edge->target->edges = n;
+		}
+		else
+		{
+			edge->target->edges = NULL;
+		}
+
+		edgePool.freeObject(edge);
+		edgePool.freeObject(r);
+		usedEdgePairs--;
+	}
+
+	void computeInternal(int start, int end, IntermediateHull& result);
+
+	bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
+
+	void merge(IntermediateHull& h0, IntermediateHull& h1);
+
+	b3Vector3 toBtVector(const Point32& v);
+
+	b3Vector3 getBtNormal(Face* face);
+
+	bool shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack);
+
+public:
+	Vertex* vertexList;
+
+	void compute(const void* coords, bool doubleCoords, int stride, int count);
+
+	b3Vector3 getCoordinates(const Vertex* v);
+
+	b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount);
+};
+
+b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::operator*(btInt64_t b) const
+{
+	bool negative = (btInt64_t)high < 0;
+	Int128 a = negative ? -*this : *this;
+	if (b < 0)
+	{
+		negative = !negative;
+		b = -b;
+	}
+	Int128 result = mul(a.low, (btUint64_t)b);
+	result.high += a.high * (btUint64_t)b;
+	return negative ? -result : result;
+}
+
+b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btInt64_t a, btInt64_t b)
+{
+	Int128 result;
+
+#ifdef USE_X86_64_ASM
+	__asm__("imulq %[b]"
+			: "=a"(result.low), "=d"(result.high)
+			: "0"(a), [b] "r"(b)
+			: "cc");
+	return result;
+
+#else
+	bool negative = a < 0;
+	if (negative)
+	{
+		a = -a;
+	}
+	if (b < 0)
+	{
+		negative = !negative;
+		b = -b;
+	}
+	DMul<btUint64_t, btUint32_t>::mul((btUint64_t)a, (btUint64_t)b, result.low, result.high);
+	return negative ? -result : result;
+#endif
+}
+
+b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btUint64_t a, btUint64_t b)
+{
+	Int128 result;
+
+#ifdef USE_X86_64_ASM
+	__asm__("mulq %[b]"
+			: "=a"(result.low), "=d"(result.high)
+			: "0"(a), [b] "r"(b)
+			: "cc");
+
+#else
+	DMul<btUint64_t, btUint32_t>::mul(a, b, result.low, result.high);
+#endif
+
+	return result;
+}
+
+int b3ConvexHullInternal::Rational64::compare(const Rational64& b) const
+{
+	if (sign != b.sign)
+	{
+		return sign - b.sign;
+	}
+	else if (sign == 0)
+	{
+		return 0;
+	}
+
+	//	return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
+
+#ifdef USE_X86_64_ASM
+
+	int result;
+	btInt64_t tmp;
+	btInt64_t dummy;
+	__asm__(
+		"mulq %[bn]\n\t"
+		"movq %%rax, %[tmp]\n\t"
+		"movq %%rdx, %%rbx\n\t"
+		"movq %[tn], %%rax\n\t"
+		"mulq %[bd]\n\t"
+		"subq %[tmp], %%rax\n\t"
+		"sbbq %%rbx, %%rdx\n\t"  // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
+		"setnsb %%bh\n\t"        // bh=1 if difference is non-negative, bh=0 otherwise
+		"orq %%rdx, %%rax\n\t"
+		"setnzb %%bl\n\t"      // bl=1 if difference if non-zero, bl=0 if it is zero
+		"decb %%bh\n\t"        // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
+		"shll $16, %%ebx\n\t"  // ebx has same sign as difference
+		: "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
+		: "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
+		: "%rdx", "cc");
+	return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
+								   // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
+				  : 0;
+
+#else
+
+	return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
+
+#endif
+}
+
+int b3ConvexHullInternal::Rational128::compare(const Rational128& b) const
+{
+	if (sign != b.sign)
+	{
+		return sign - b.sign;
+	}
+	else if (sign == 0)
+	{
+		return 0;
+	}
+	if (isInt64)
+	{
+		return -b.compare(sign * (btInt64_t)numerator.low);
+	}
+
+	Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
+	DMul<Int128, btUint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
+	DMul<Int128, btUint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
+
+	int cmp = nbdHigh.ucmp(dbnHigh);
+	if (cmp)
+	{
+		return cmp * sign;
+	}
+	return nbdLow.ucmp(dbnLow) * sign;
+}
+
+int b3ConvexHullInternal::Rational128::compare(btInt64_t b) const
+{
+	if (isInt64)
+	{
+		btInt64_t a = sign * (btInt64_t)numerator.low;
+		return (a > b) ? 1 : (a < b) ? -1 : 0;
+	}
+	if (b > 0)
+	{
+		if (sign <= 0)
+		{
+			return -1;
+		}
+	}
+	else if (b < 0)
+	{
+		if (sign >= 0)
+		{
+			return 1;
+		}
+		b = -b;
+	}
+	else
+	{
+		return sign;
+	}
+
+	return numerator.ucmp(denominator * b) * sign;
+}
+
+b3ConvexHullInternal::Edge* b3ConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
+{
+	b3Assert(from && to);
+	Edge* e = edgePool.newObject();
+	Edge* r = edgePool.newObject();
+	e->reverse = r;
+	r->reverse = e;
+	e->copy = mergeStamp;
+	r->copy = mergeStamp;
+	e->target = to;
+	r->target = from;
+	e->face = NULL;
+	r->face = NULL;
+	usedEdgePairs++;
+	if (usedEdgePairs > maxUsedEdgePairs)
+	{
+		maxUsedEdgePairs = usedEdgePairs;
+	}
+	return e;
+}
+
+bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
+{
+	Vertex* v0 = h0.maxYx;
+	Vertex* v1 = h1.minYx;
+	if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
+	{
+		b3Assert(v0->point.z < v1->point.z);
+		Vertex* v1p = v1->prev;
+		if (v1p == v1)
+		{
+			c0 = v0;
+			if (v1->edges)
+			{
+				b3Assert(v1->edges->next == v1->edges);
+				v1 = v1->edges->target;
+				b3Assert(v1->edges->next == v1->edges);
+			}
+			c1 = v1;
+			return false;
+		}
+		Vertex* v1n = v1->next;
+		v1p->next = v1n;
+		v1n->prev = v1p;
+		if (v1 == h1.minXy)
+		{
+			if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y)))
+			{
+				h1.minXy = v1n;
+			}
+			else
+			{
+				h1.minXy = v1p;
+			}
+		}
+		if (v1 == h1.maxXy)
+		{
+			if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y)))
+			{
+				h1.maxXy = v1n;
+			}
+			else
+			{
+				h1.maxXy = v1p;
+			}
+		}
+	}
+
+	v0 = h0.maxXy;
+	v1 = h1.maxXy;
+	Vertex* v00 = NULL;
+	Vertex* v10 = NULL;
+	btInt32_t sign = 1;
+
+	for (int side = 0; side <= 1; side++)
+	{
+		btInt32_t dx = (v1->point.x - v0->point.x) * sign;
+		if (dx > 0)
+		{
+			while (true)
+			{
+				btInt32_t dy = v1->point.y - v0->point.y;
+
+				Vertex* w0 = side ? v0->next : v0->prev;
+				if (w0 != v0)
+				{
+					btInt32_t dx0 = (w0->point.x - v0->point.x) * sign;
+					btInt32_t dy0 = w0->point.y - v0->point.y;
+					if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0))))
+					{
+						v0 = w0;
+						dx = (v1->point.x - v0->point.x) * sign;
+						continue;
+					}
+				}
+
+				Vertex* w1 = side ? v1->next : v1->prev;
+				if (w1 != v1)
+				{
+					btInt32_t dx1 = (w1->point.x - v1->point.x) * sign;
+					btInt32_t dy1 = w1->point.y - v1->point.y;
+					btInt32_t dxn = (w1->point.x - v0->point.x) * sign;
+					if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1))))
+					{
+						v1 = w1;
+						dx = dxn;
+						continue;
+					}
+				}
+
+				break;
+			}
+		}
+		else if (dx < 0)
+		{
+			while (true)
+			{
+				btInt32_t dy = v1->point.y - v0->point.y;
+
+				Vertex* w1 = side ? v1->prev : v1->next;
+				if (w1 != v1)
+				{
+					btInt32_t dx1 = (w1->point.x - v1->point.x) * sign;
+					btInt32_t dy1 = w1->point.y - v1->point.y;
+					if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1))))
+					{
+						v1 = w1;
+						dx = (v1->point.x - v0->point.x) * sign;
+						continue;
+					}
+				}
+
+				Vertex* w0 = side ? v0->prev : v0->next;
+				if (w0 != v0)
+				{
+					btInt32_t dx0 = (w0->point.x - v0->point.x) * sign;
+					btInt32_t dy0 = w0->point.y - v0->point.y;
+					btInt32_t dxn = (v1->point.x - w0->point.x) * sign;
+					if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0))))
+					{
+						v0 = w0;
+						dx = dxn;
+						continue;
+					}
+				}
+
+				break;
+			}
+		}
+		else
+		{
+			btInt32_t x = v0->point.x;
+			btInt32_t y0 = v0->point.y;
+			Vertex* w0 = v0;
+			Vertex* t;
+			while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0))
+			{
+				w0 = t;
+				y0 = t->point.y;
+			}
+			v0 = w0;
+
+			btInt32_t y1 = v1->point.y;
+			Vertex* w1 = v1;
+			while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1))
+			{
+				w1 = t;
+				y1 = t->point.y;
+			}
+			v1 = w1;
+		}
+
+		if (side == 0)
+		{
+			v00 = v0;
+			v10 = v1;
+
+			v0 = h0.minXy;
+			v1 = h1.minXy;
+			sign = -1;
+		}
+	}
+
+	v0->prev = v1;
+	v1->next = v0;
+
+	v00->next = v10;
+	v10->prev = v00;
+
+	if (h1.minXy->point.x < h0.minXy->point.x)
+	{
+		h0.minXy = h1.minXy;
+	}
+	if (h1.maxXy->point.x >= h0.maxXy->point.x)
+	{
+		h0.maxXy = h1.maxXy;
+	}
+
+	h0.maxYx = h1.maxYx;
+
+	c0 = v00;
+	c1 = v10;
+
+	return true;
+}
+
+void b3ConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
+{
+	int n = end - start;
+	switch (n)
+	{
+		case 0:
+			result.minXy = NULL;
+			result.maxXy = NULL;
+			result.minYx = NULL;
+			result.maxYx = NULL;
+			return;
+		case 2:
+		{
+			Vertex* v = originalVertices[start];
+			Vertex* w = v + 1;
+			if (v->point != w->point)
+			{
+				btInt32_t dx = v->point.x - w->point.x;
+				btInt32_t dy = v->point.y - w->point.y;
+
+				if ((dx == 0) && (dy == 0))
+				{
+					if (v->point.z > w->point.z)
+					{
+						Vertex* t = w;
+						w = v;
+						v = t;
+					}
+					b3Assert(v->point.z < w->point.z);
+					v->next = v;
+					v->prev = v;
+					result.minXy = v;
+					result.maxXy = v;
+					result.minYx = v;
+					result.maxYx = v;
+				}
+				else
+				{
+					v->next = w;
+					v->prev = w;
+					w->next = v;
+					w->prev = v;
+
+					if ((dx < 0) || ((dx == 0) && (dy < 0)))
+					{
+						result.minXy = v;
+						result.maxXy = w;
+					}
+					else
+					{
+						result.minXy = w;
+						result.maxXy = v;
+					}
+
+					if ((dy < 0) || ((dy == 0) && (dx < 0)))
+					{
+						result.minYx = v;
+						result.maxYx = w;
+					}
+					else
+					{
+						result.minYx = w;
+						result.maxYx = v;
+					}
+				}
+
+				Edge* e = newEdgePair(v, w);
+				e->link(e);
+				v->edges = e;
+
+				e = e->reverse;
+				e->link(e);
+				w->edges = e;
+
+				return;
+			}
+		}
+		// lint -fallthrough
+		case 1:
+		{
+			Vertex* v = originalVertices[start];
+			v->edges = NULL;
+			v->next = v;
+			v->prev = v;
+
+			result.minXy = v;
+			result.maxXy = v;
+			result.minYx = v;
+			result.maxYx = v;
+
+			return;
+		}
+	}
+
+	int split0 = start + n / 2;
+	Point32 p = originalVertices[split0 - 1]->point;
+	int split1 = split0;
+	while ((split1 < end) && (originalVertices[split1]->point == p))
+	{
+		split1++;
+	}
+	computeInternal(start, split0, result);
+	IntermediateHull hull1;
+	computeInternal(split1, end, hull1);
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("\n\nMerge\n");
+	result.print();
+	hull1.print();
+#endif
+	merge(result, hull1);
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("\n  Result\n");
+	result.print();
+#endif
+}
+
+#ifdef DEBUG_CONVEX_HULL
+void b3ConvexHullInternal::IntermediateHull::print()
+{
+	b3Printf("    Hull\n");
+	for (Vertex* v = minXy; v;)
+	{
+		b3Printf("      ");
+		v->print();
+		if (v == maxXy)
+		{
+			b3Printf(" maxXy");
+		}
+		if (v == minYx)
+		{
+			b3Printf(" minYx");
+		}
+		if (v == maxYx)
+		{
+			b3Printf(" maxYx");
+		}
+		if (v->next->prev != v)
+		{
+			b3Printf(" Inconsistency");
+		}
+		b3Printf("\n");
+		v = v->next;
+		if (v == minXy)
+		{
+			break;
+		}
+	}
+	if (minXy)
+	{
+		minXy->copy = (minXy->copy == -1) ? -2 : -1;
+		minXy->printGraph();
+	}
+}
+
+void b3ConvexHullInternal::Vertex::printGraph()
+{
+	print();
+	b3Printf("\nEdges\n");
+	Edge* e = edges;
+	if (e)
+	{
+		do
+		{
+			e->print();
+			b3Printf("\n");
+			e = e->next;
+		} while (e != edges);
+		do
+		{
+			Vertex* v = e->target;
+			if (v->copy != copy)
+			{
+				v->copy = copy;
+				v->printGraph();
+			}
+			e = e->next;
+		} while (e != edges);
+	}
+}
+#endif
+
+b3ConvexHullInternal::Orientation b3ConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
+{
+	b3Assert(prev->reverse->target == next->reverse->target);
+	if (prev->next == next)
+	{
+		if (prev->prev == next)
+		{
+			Point64 n = t.cross(s);
+			Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
+			b3Assert(!m.isZero());
+			btInt64_t dot = n.dot(m);
+			b3Assert(dot != 0);
+			return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
+		}
+		return COUNTER_CLOCKWISE;
+	}
+	else if (prev->prev == next)
+	{
+		return CLOCKWISE;
+	}
+	else
+	{
+		return NONE;
+	}
+}
+
+b3ConvexHullInternal::Edge* b3ConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
+{
+	Edge* minEdge = NULL;
+
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("find max edge for %d\n", start->point.index);
+#endif
+	Edge* e = start->edges;
+	if (e)
+	{
+		do
+		{
+			if (e->copy > mergeStamp)
+			{
+				Point32 t = *e->target - *start;
+				Rational64 cot(t.dot(sxrxs), t.dot(rxs));
+#ifdef DEBUG_CONVEX_HULL
+				b3Printf("      Angle is %f (%d) for ", (float)b3Atan(cot.toScalar()), (int)cot.isNaN());
+				e->print();
+#endif
+				if (cot.isNaN())
+				{
+					b3Assert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
+				}
+				else
+				{
+					int cmp;
+					if (minEdge == NULL)
+					{
+						minCot = cot;
+						minEdge = e;
+					}
+					else if ((cmp = cot.compare(minCot)) < 0)
+					{
+						minCot = cot;
+						minEdge = e;
+					}
+					else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE)))
+					{
+						minEdge = e;
+					}
+				}
+#ifdef DEBUG_CONVEX_HULL
+				b3Printf("\n");
+#endif
+			}
+			e = e->next;
+		} while (e != start->edges);
+	}
+	return minEdge;
+}
+
+void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
+{
+	Edge* start0 = e0;
+	Edge* start1 = e1;
+	Point32 et0 = start0 ? start0->target->point : c0->point;
+	Point32 et1 = start1 ? start1->target->point : c1->point;
+	Point32 s = c1->point - c0->point;
+	Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
+	btInt64_t dist = c0->point.dot(normal);
+	b3Assert(!start1 || (start1->target->point.dot(normal) == dist));
+	Point64 perp = s.cross(normal);
+	b3Assert(!perp.isZero());
+
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("   Advancing %d %d  (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
+#endif
+
+	btInt64_t maxDot0 = et0.dot(perp);
+	if (e0)
+	{
+		while (e0->target != stop0)
+		{
+			Edge* e = e0->reverse->prev;
+			if (e->target->point.dot(normal) < dist)
+			{
+				break;
+			}
+			b3Assert(e->target->point.dot(normal) == dist);
+			if (e->copy == mergeStamp)
+			{
+				break;
+			}
+			btInt64_t dot = e->target->point.dot(perp);
+			if (dot <= maxDot0)
+			{
+				break;
+			}
+			maxDot0 = dot;
+			e0 = e;
+			et0 = e->target->point;
+		}
+	}
+
+	btInt64_t maxDot1 = et1.dot(perp);
+	if (e1)
+	{
+		while (e1->target != stop1)
+		{
+			Edge* e = e1->reverse->next;
+			if (e->target->point.dot(normal) < dist)
+			{
+				break;
+			}
+			b3Assert(e->target->point.dot(normal) == dist);
+			if (e->copy == mergeStamp)
+			{
+				break;
+			}
+			btInt64_t dot = e->target->point.dot(perp);
+			if (dot <= maxDot1)
+			{
+				break;
+			}
+			maxDot1 = dot;
+			e1 = e;
+			et1 = e->target->point;
+		}
+	}
+
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("   Starting at %d %d\n", et0.index, et1.index);
+#endif
+
+	btInt64_t dx = maxDot1 - maxDot0;
+	if (dx > 0)
+	{
+		while (true)
+		{
+			btInt64_t dy = (et1 - et0).dot(s);
+
+			if (e0 && (e0->target != stop0))
+			{
+				Edge* f0 = e0->next->reverse;
+				if (f0->copy > mergeStamp)
+				{
+					btInt64_t dx0 = (f0->target->point - et0).dot(perp);
+					btInt64_t dy0 = (f0->target->point - et0).dot(s);
+					if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0)))
+					{
+						et0 = f0->target->point;
+						dx = (et1 - et0).dot(perp);
+						e0 = (e0 == start0) ? NULL : f0;
+						continue;
+					}
+				}
+			}
+
+			if (e1 && (e1->target != stop1))
+			{
+				Edge* f1 = e1->reverse->next;
+				if (f1->copy > mergeStamp)
+				{
+					Point32 d1 = f1->target->point - et1;
+					if (d1.dot(normal) == 0)
+					{
+						btInt64_t dx1 = d1.dot(perp);
+						btInt64_t dy1 = d1.dot(s);
+						btInt64_t dxn = (f1->target->point - et0).dot(perp);
+						if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0))))
+						{
+							e1 = f1;
+							et1 = e1->target->point;
+							dx = dxn;
+							continue;
+						}
+					}
+					else
+					{
+						b3Assert((e1 == start1) && (d1.dot(normal) < 0));
+					}
+				}
+			}
+
+			break;
+		}
+	}
+	else if (dx < 0)
+	{
+		while (true)
+		{
+			btInt64_t dy = (et1 - et0).dot(s);
+
+			if (e1 && (e1->target != stop1))
+			{
+				Edge* f1 = e1->prev->reverse;
+				if (f1->copy > mergeStamp)
+				{
+					btInt64_t dx1 = (f1->target->point - et1).dot(perp);
+					btInt64_t dy1 = (f1->target->point - et1).dot(s);
+					if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0)))
+					{
+						et1 = f1->target->point;
+						dx = (et1 - et0).dot(perp);
+						e1 = (e1 == start1) ? NULL : f1;
+						continue;
+					}
+				}
+			}
+
+			if (e0 && (e0->target != stop0))
+			{
+				Edge* f0 = e0->reverse->prev;
+				if (f0->copy > mergeStamp)
+				{
+					Point32 d0 = f0->target->point - et0;
+					if (d0.dot(normal) == 0)
+					{
+						btInt64_t dx0 = d0.dot(perp);
+						btInt64_t dy0 = d0.dot(s);
+						btInt64_t dxn = (et1 - f0->target->point).dot(perp);
+						if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0))))
+						{
+							e0 = f0;
+							et0 = e0->target->point;
+							dx = dxn;
+							continue;
+						}
+					}
+					else
+					{
+						b3Assert((e0 == start0) && (d0.dot(normal) < 0));
+					}
+				}
+			}
+
+			break;
+		}
+	}
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("   Advanced edges to %d %d\n", et0.index, et1.index);
+#endif
+}
+
+void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
+{
+	if (!h1.maxXy)
+	{
+		return;
+	}
+	if (!h0.maxXy)
+	{
+		h0 = h1;
+		return;
+	}
+
+	mergeStamp--;
+
+	Vertex* c0 = NULL;
+	Edge* toPrev0 = NULL;
+	Edge* firstNew0 = NULL;
+	Edge* pendingHead0 = NULL;
+	Edge* pendingTail0 = NULL;
+	Vertex* c1 = NULL;
+	Edge* toPrev1 = NULL;
+	Edge* firstNew1 = NULL;
+	Edge* pendingHead1 = NULL;
+	Edge* pendingTail1 = NULL;
+	Point32 prevPoint;
+
+	if (mergeProjection(h0, h1, c0, c1))
+	{
+		Point32 s = *c1 - *c0;
+		Point64 normal = Point32(0, 0, -1).cross(s);
+		Point64 t = s.cross(normal);
+		b3Assert(!t.isZero());
+
+		Edge* e = c0->edges;
+		Edge* start0 = NULL;
+		if (e)
+		{
+			do
+			{
+				btInt64_t dot = (*e->target - *c0).dot(normal);
+				b3Assert(dot <= 0);
+				if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
+				{
+					if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
+					{
+						start0 = e;
+					}
+				}
+				e = e->next;
+			} while (e != c0->edges);
+		}
+
+		e = c1->edges;
+		Edge* start1 = NULL;
+		if (e)
+		{
+			do
+			{
+				btInt64_t dot = (*e->target - *c1).dot(normal);
+				b3Assert(dot <= 0);
+				if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
+				{
+					if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
+					{
+						start1 = e;
+					}
+				}
+				e = e->next;
+			} while (e != c1->edges);
+		}
+
+		if (start0 || start1)
+		{
+			findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
+			if (start0)
+			{
+				c0 = start0->target;
+			}
+			if (start1)
+			{
+				c1 = start1->target;
+			}
+		}
+
+		prevPoint = c1->point;
+		prevPoint.z++;
+	}
+	else
+	{
+		prevPoint = c1->point;
+		prevPoint.x++;
+	}
+
+	Vertex* first0 = c0;
+	Vertex* first1 = c1;
+	bool firstRun = true;
+
+	while (true)
+	{
+		Point32 s = *c1 - *c0;
+		Point32 r = prevPoint - c0->point;
+		Point64 rxs = r.cross(s);
+		Point64 sxrxs = s.cross(rxs);
+
+#ifdef DEBUG_CONVEX_HULL
+		b3Printf("\n  Checking %d %d\n", c0->point.index, c1->point.index);
+#endif
+		Rational64 minCot0(0, 0);
+		Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
+		Rational64 minCot1(0, 0);
+		Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
+		if (!min0 && !min1)
+		{
+			Edge* e = newEdgePair(c0, c1);
+			e->link(e);
+			c0->edges = e;
+
+			e = e->reverse;
+			e->link(e);
+			c1->edges = e;
+			return;
+		}
+		else
+		{
+			int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("    -> Result %d\n", cmp);
+#endif
+			if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity()))
+			{
+				Edge* e = newEdgePair(c0, c1);
+				if (pendingTail0)
+				{
+					pendingTail0->prev = e;
+				}
+				else
+				{
+					pendingHead0 = e;
+				}
+				e->next = pendingTail0;
+				pendingTail0 = e;
+
+				e = e->reverse;
+				if (pendingTail1)
+				{
+					pendingTail1->next = e;
+				}
+				else
+				{
+					pendingHead1 = e;
+				}
+				e->prev = pendingTail1;
+				pendingTail1 = e;
+			}
+
+			Edge* e0 = min0;
+			Edge* e1 = min1;
+
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("   Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
+#endif
+
+			if (cmp == 0)
+			{
+				findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
+			}
+
+			if ((cmp >= 0) && e1)
+			{
+				if (toPrev1)
+				{
+					for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n)
+					{
+						n = e->next;
+						removeEdgePair(e);
+					}
+				}
+
+				if (pendingTail1)
+				{
+					if (toPrev1)
+					{
+						toPrev1->link(pendingHead1);
+					}
+					else
+					{
+						min1->prev->link(pendingHead1);
+						firstNew1 = pendingHead1;
+					}
+					pendingTail1->link(min1);
+					pendingHead1 = NULL;
+					pendingTail1 = NULL;
+				}
+				else if (!toPrev1)
+				{
+					firstNew1 = min1;
+				}
+
+				prevPoint = c1->point;
+				c1 = e1->target;
+				toPrev1 = e1->reverse;
+			}
+
+			if ((cmp <= 0) && e0)
+			{
+				if (toPrev0)
+				{
+					for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n)
+					{
+						n = e->prev;
+						removeEdgePair(e);
+					}
+				}
+
+				if (pendingTail0)
+				{
+					if (toPrev0)
+					{
+						pendingHead0->link(toPrev0);
+					}
+					else
+					{
+						pendingHead0->link(min0->next);
+						firstNew0 = pendingHead0;
+					}
+					min0->link(pendingTail0);
+					pendingHead0 = NULL;
+					pendingTail0 = NULL;
+				}
+				else if (!toPrev0)
+				{
+					firstNew0 = min0;
+				}
+
+				prevPoint = c0->point;
+				c0 = e0->target;
+				toPrev0 = e0->reverse;
+			}
+		}
+
+		if ((c0 == first0) && (c1 == first1))
+		{
+			if (toPrev0 == NULL)
+			{
+				pendingHead0->link(pendingTail0);
+				c0->edges = pendingTail0;
+			}
+			else
+			{
+				for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n)
+				{
+					n = e->prev;
+					removeEdgePair(e);
+				}
+				if (pendingTail0)
+				{
+					pendingHead0->link(toPrev0);
+					firstNew0->link(pendingTail0);
+				}
+			}
+
+			if (toPrev1 == NULL)
+			{
+				pendingTail1->link(pendingHead1);
+				c1->edges = pendingTail1;
+			}
+			else
+			{
+				for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n)
+				{
+					n = e->next;
+					removeEdgePair(e);
+				}
+				if (pendingTail1)
+				{
+					toPrev1->link(pendingHead1);
+					pendingTail1->link(firstNew1);
+				}
+			}
+
+			return;
+		}
+
+		firstRun = false;
+	}
+}
+
+static bool b3PointCmp(const b3ConvexHullInternal::Point32& p, const b3ConvexHullInternal::Point32& q)
+{
+	return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
+}
+
+void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
+{
+	b3Vector3 min = b3MakeVector3(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max = b3MakeVector3(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30));
+	const char* ptr = (const char*)coords;
+	if (doubleCoords)
+	{
+		for (int i = 0; i < count; i++)
+		{
+			const double* v = (const double*)ptr;
+			b3Vector3 p = b3MakeVector3((b3Scalar)v[0], (b3Scalar)v[1], (b3Scalar)v[2]);
+			ptr += stride;
+			min.setMin(p);
+			max.setMax(p);
+		}
+	}
+	else
+	{
+		for (int i = 0; i < count; i++)
+		{
+			const float* v = (const float*)ptr;
+			b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]);
+			ptr += stride;
+			min.setMin(p);
+			max.setMax(p);
+		}
+	}
+
+	b3Vector3 s = max - min;
+	maxAxis = s.maxAxis();
+	minAxis = s.minAxis();
+	if (minAxis == maxAxis)
+	{
+		minAxis = (maxAxis + 1) % 3;
+	}
+	medAxis = 3 - maxAxis - minAxis;
+
+	s /= b3Scalar(10216);
+	if (((medAxis + 1) % 3) != maxAxis)
+	{
+		s *= -1;
+	}
+	scaling = s;
+
+	if (s[0] != 0)
+	{
+		s[0] = b3Scalar(1) / s[0];
+	}
+	if (s[1] != 0)
+	{
+		s[1] = b3Scalar(1) / s[1];
+	}
+	if (s[2] != 0)
+	{
+		s[2] = b3Scalar(1) / s[2];
+	}
+
+	center = (min + max) * b3Scalar(0.5);
+
+	b3AlignedObjectArray<Point32> points;
+	points.resize(count);
+	ptr = (const char*)coords;
+	if (doubleCoords)
+	{
+		for (int i = 0; i < count; i++)
+		{
+			const double* v = (const double*)ptr;
+			b3Vector3 p = b3MakeVector3((b3Scalar)v[0], (b3Scalar)v[1], (b3Scalar)v[2]);
+			ptr += stride;
+			p = (p - center) * s;
+			points[i].x = (btInt32_t)p[medAxis];
+			points[i].y = (btInt32_t)p[maxAxis];
+			points[i].z = (btInt32_t)p[minAxis];
+			points[i].index = i;
+		}
+	}
+	else
+	{
+		for (int i = 0; i < count; i++)
+		{
+			const float* v = (const float*)ptr;
+			b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]);
+			ptr += stride;
+			p = (p - center) * s;
+			points[i].x = (btInt32_t)p[medAxis];
+			points[i].y = (btInt32_t)p[maxAxis];
+			points[i].z = (btInt32_t)p[minAxis];
+			points[i].index = i;
+		}
+	}
+	points.quickSort(b3PointCmp);
+
+	vertexPool.reset();
+	vertexPool.setArraySize(count);
+	originalVertices.resize(count);
+	for (int i = 0; i < count; i++)
+	{
+		Vertex* v = vertexPool.newObject();
+		v->edges = NULL;
+		v->point = points[i];
+		v->copy = -1;
+		originalVertices[i] = v;
+	}
+
+	points.clear();
+
+	edgePool.reset();
+	edgePool.setArraySize(6 * count);
+
+	usedEdgePairs = 0;
+	maxUsedEdgePairs = 0;
+
+	mergeStamp = -3;
+
+	IntermediateHull hull;
+	computeInternal(0, count, hull);
+	vertexList = hull.minXy;
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
+#endif
+}
+
+b3Vector3 b3ConvexHullInternal::toBtVector(const Point32& v)
+{
+	b3Vector3 p;
+	p[medAxis] = b3Scalar(v.x);
+	p[maxAxis] = b3Scalar(v.y);
+	p[minAxis] = b3Scalar(v.z);
+	return p * scaling;
+}
+
+b3Vector3 b3ConvexHullInternal::getBtNormal(Face* face)
+{
+	return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
+}
+
+b3Vector3 b3ConvexHullInternal::getCoordinates(const Vertex* v)
+{
+	b3Vector3 p;
+	p[medAxis] = v->xvalue();
+	p[maxAxis] = v->yvalue();
+	p[minAxis] = v->zvalue();
+	return p * scaling + center;
+}
+
+b3Scalar b3ConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
+{
+	if (!vertexList)
+	{
+		return 0;
+	}
+	int stamp = --mergeStamp;
+	b3AlignedObjectArray<Vertex*> stack;
+	vertexList->copy = stamp;
+	stack.push_back(vertexList);
+	b3AlignedObjectArray<Face*> faces;
+
+	Point32 ref = vertexList->point;
+	Int128 hullCenterX(0, 0);
+	Int128 hullCenterY(0, 0);
+	Int128 hullCenterZ(0, 0);
+	Int128 volume(0, 0);
+
+	while (stack.size() > 0)
+	{
+		Vertex* v = stack[stack.size() - 1];
+		stack.pop_back();
+		Edge* e = v->edges;
+		if (e)
+		{
+			do
+			{
+				if (e->target->copy != stamp)
+				{
+					e->target->copy = stamp;
+					stack.push_back(e->target);
+				}
+				if (e->copy != stamp)
+				{
+					Face* face = facePool.newObject();
+					face->init(e->target, e->reverse->prev->target, v);
+					faces.push_back(face);
+					Edge* f = e;
+
+					Vertex* a = NULL;
+					Vertex* b = NULL;
+					do
+					{
+						if (a && b)
+						{
+							btInt64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
+							b3Assert(vol >= 0);
+							Point32 c = v->point + a->point + b->point + ref;
+							hullCenterX += vol * c.x;
+							hullCenterY += vol * c.y;
+							hullCenterZ += vol * c.z;
+							volume += vol;
+						}
+
+						b3Assert(f->copy != stamp);
+						f->copy = stamp;
+						f->face = face;
+
+						a = b;
+						b = f->target;
+
+						f = f->reverse->prev;
+					} while (f != e);
+				}
+				e = e->next;
+			} while (e != v->edges);
+		}
+	}
+
+	if (volume.getSign() <= 0)
+	{
+		return 0;
+	}
+
+	b3Vector3 hullCenter;
+	hullCenter[medAxis] = hullCenterX.toScalar();
+	hullCenter[maxAxis] = hullCenterY.toScalar();
+	hullCenter[minAxis] = hullCenterZ.toScalar();
+	hullCenter /= 4 * volume.toScalar();
+	hullCenter *= scaling;
+
+	int faceCount = faces.size();
+
+	if (clampAmount > 0)
+	{
+		b3Scalar minDist = B3_INFINITY;
+		for (int i = 0; i < faceCount; i++)
+		{
+			b3Vector3 normal = getBtNormal(faces[i]);
+			b3Scalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
+			if (dist < minDist)
+			{
+				minDist = dist;
+			}
+		}
+
+		if (minDist <= 0)
+		{
+			return 0;
+		}
+
+		amount = b3Min(amount, minDist * clampAmount);
+	}
+
+	unsigned int seed = 243703;
+	for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
+	{
+		b3Swap(faces[i], faces[seed % faceCount]);
+	}
+
+	for (int i = 0; i < faceCount; i++)
+	{
+		if (!shiftFace(faces[i], amount, stack))
+		{
+			return -amount;
+		}
+	}
+
+	return amount;
+}
+
+bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack)
+{
+	b3Vector3 origShift = getBtNormal(face) * -amount;
+	if (scaling[0] != 0)
+	{
+		origShift[0] /= scaling[0];
+	}
+	if (scaling[1] != 0)
+	{
+		origShift[1] /= scaling[1];
+	}
+	if (scaling[2] != 0)
+	{
+		origShift[2] /= scaling[2];
+	}
+	Point32 shift((btInt32_t)origShift[medAxis], (btInt32_t)origShift[maxAxis], (btInt32_t)origShift[minAxis]);
+	if (shift.isZero())
+	{
+		return true;
+	}
+	Point64 normal = face->getNormal();
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
+			 face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
+#endif
+	btInt64_t origDot = face->origin.dot(normal);
+	Point32 shiftedOrigin = face->origin + shift;
+	btInt64_t shiftedDot = shiftedOrigin.dot(normal);
+	b3Assert(shiftedDot <= origDot);
+	if (shiftedDot >= origDot)
+	{
+		return false;
+	}
+
+	Edge* intersection = NULL;
+
+	Edge* startEdge = face->nearbyVertex->edges;
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("Start edge is ");
+	startEdge->print();
+	b3Printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
+#endif
+	Rational128 optDot = face->nearbyVertex->dot(normal);
+	int cmp = optDot.compare(shiftedDot);
+#ifdef SHOW_ITERATIONS
+	int n = 0;
+#endif
+	if (cmp >= 0)
+	{
+		Edge* e = startEdge;
+		do
+		{
+#ifdef SHOW_ITERATIONS
+			n++;
+#endif
+			Rational128 dot = e->target->dot(normal);
+			b3Assert(dot.compare(origDot) <= 0);
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("Moving downwards, edge is ");
+			e->print();
+			b3Printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
+#endif
+			if (dot.compare(optDot) < 0)
+			{
+				int c = dot.compare(shiftedDot);
+				optDot = dot;
+				e = e->reverse;
+				startEdge = e;
+				if (c < 0)
+				{
+					intersection = e;
+					break;
+				}
+				cmp = c;
+			}
+			e = e->prev;
+		} while (e != startEdge);
+
+		if (!intersection)
+		{
+			return false;
+		}
+	}
+	else
+	{
+		Edge* e = startEdge;
+		do
+		{
+#ifdef SHOW_ITERATIONS
+			n++;
+#endif
+			Rational128 dot = e->target->dot(normal);
+			b3Assert(dot.compare(origDot) <= 0);
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("Moving upwards, edge is ");
+			e->print();
+			b3Printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
+#endif
+			if (dot.compare(optDot) > 0)
+			{
+				cmp = dot.compare(shiftedDot);
+				if (cmp >= 0)
+				{
+					intersection = e;
+					break;
+				}
+				optDot = dot;
+				e = e->reverse;
+				startEdge = e;
+			}
+			e = e->prev;
+		} while (e != startEdge);
+
+		if (!intersection)
+		{
+			return true;
+		}
+	}
+
+#ifdef SHOW_ITERATIONS
+	b3Printf("Needed %d iterations to find initial intersection\n", n);
+#endif
+
+	if (cmp == 0)
+	{
+		Edge* e = intersection->reverse->next;
+#ifdef SHOW_ITERATIONS
+		n = 0;
+#endif
+		while (e->target->dot(normal).compare(shiftedDot) <= 0)
+		{
+#ifdef SHOW_ITERATIONS
+			n++;
+#endif
+			e = e->next;
+			if (e == intersection->reverse)
+			{
+				return true;
+			}
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("Checking for outwards edge, current edge is ");
+			e->print();
+			b3Printf("\n");
+#endif
+		}
+#ifdef SHOW_ITERATIONS
+		b3Printf("Needed %d iterations to check for complete containment\n", n);
+#endif
+	}
+
+	Edge* firstIntersection = NULL;
+	Edge* faceEdge = NULL;
+	Edge* firstFaceEdge = NULL;
+
+#ifdef SHOW_ITERATIONS
+	int m = 0;
+#endif
+	while (true)
+	{
+#ifdef SHOW_ITERATIONS
+		m++;
+#endif
+#ifdef DEBUG_CONVEX_HULL
+		b3Printf("Intersecting edge is ");
+		intersection->print();
+		b3Printf("\n");
+#endif
+		if (cmp == 0)
+		{
+			Edge* e = intersection->reverse->next;
+			startEdge = e;
+#ifdef SHOW_ITERATIONS
+			n = 0;
+#endif
+			while (true)
+			{
+#ifdef SHOW_ITERATIONS
+				n++;
+#endif
+				if (e->target->dot(normal).compare(shiftedDot) >= 0)
+				{
+					break;
+				}
+				intersection = e->reverse;
+				e = e->next;
+				if (e == startEdge)
+				{
+					return true;
+				}
+			}
+#ifdef SHOW_ITERATIONS
+			b3Printf("Needed %d iterations to advance intersection\n", n);
+#endif
+		}
+
+#ifdef DEBUG_CONVEX_HULL
+		b3Printf("Advanced intersecting edge to ");
+		intersection->print();
+		b3Printf(", cmp = %d\n", cmp);
+#endif
+
+		if (!firstIntersection)
+		{
+			firstIntersection = intersection;
+		}
+		else if (intersection == firstIntersection)
+		{
+			break;
+		}
+
+		int prevCmp = cmp;
+		Edge* prevIntersection = intersection;
+		Edge* prevFaceEdge = faceEdge;
+
+		Edge* e = intersection->reverse;
+#ifdef SHOW_ITERATIONS
+		n = 0;
+#endif
+		while (true)
+		{
+#ifdef SHOW_ITERATIONS
+			n++;
+#endif
+			e = e->reverse->prev;
+			b3Assert(e != intersection->reverse);
+			cmp = e->target->dot(normal).compare(shiftedDot);
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("Testing edge ");
+			e->print();
+			b3Printf(" -> cmp = %d\n", cmp);
+#endif
+			if (cmp >= 0)
+			{
+				intersection = e;
+				break;
+			}
+		}
+#ifdef SHOW_ITERATIONS
+		b3Printf("Needed %d iterations to find other intersection of face\n", n);
+#endif
+
+		if (cmp > 0)
+		{
+			Vertex* removed = intersection->target;
+			e = intersection->reverse;
+			if (e->prev == e)
+			{
+				removed->edges = NULL;
+			}
+			else
+			{
+				removed->edges = e->prev;
+				e->prev->link(e->next);
+				e->link(e);
+			}
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+
+			Point64 n0 = intersection->face->getNormal();
+			Point64 n1 = intersection->reverse->face->getNormal();
+			btInt64_t m00 = face->dir0.dot(n0);
+			btInt64_t m01 = face->dir1.dot(n0);
+			btInt64_t m10 = face->dir0.dot(n1);
+			btInt64_t m11 = face->dir1.dot(n1);
+			btInt64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
+			btInt64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
+			Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
+			b3Assert(det.getSign() != 0);
+			Vertex* v = vertexPool.newObject();
+			v->point.index = -1;
+			v->copy = -1;
+			v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
+									Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
+									Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
+									det);
+			v->point.x = (btInt32_t)v->point128.xvalue();
+			v->point.y = (btInt32_t)v->point128.yvalue();
+			v->point.z = (btInt32_t)v->point128.zvalue();
+			intersection->target = v;
+			v->edges = e;
+
+			stack.push_back(v);
+			stack.push_back(removed);
+			stack.push_back(NULL);
+		}
+
+		if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target))
+		{
+			faceEdge = newEdgePair(prevIntersection->target, intersection->target);
+			if (prevCmp == 0)
+			{
+				faceEdge->link(prevIntersection->reverse->next);
+			}
+			if ((prevCmp == 0) || prevFaceEdge)
+			{
+				prevIntersection->reverse->link(faceEdge);
+			}
+			if (cmp == 0)
+			{
+				intersection->reverse->prev->link(faceEdge->reverse);
+			}
+			faceEdge->reverse->link(intersection->reverse);
+		}
+		else
+		{
+			faceEdge = prevIntersection->reverse->next;
+		}
+
+		if (prevFaceEdge)
+		{
+			if (prevCmp > 0)
+			{
+				faceEdge->link(prevFaceEdge->reverse);
+			}
+			else if (faceEdge != prevFaceEdge->reverse)
+			{
+				stack.push_back(prevFaceEdge->target);
+				while (faceEdge->next != prevFaceEdge->reverse)
+				{
+					Vertex* removed = faceEdge->next->target;
+					removeEdgePair(faceEdge->next);
+					stack.push_back(removed);
+#ifdef DEBUG_CONVEX_HULL
+					b3Printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+				}
+				stack.push_back(NULL);
+			}
+		}
+		faceEdge->face = face;
+		faceEdge->reverse->face = intersection->face;
+
+		if (!firstFaceEdge)
+		{
+			firstFaceEdge = faceEdge;
+		}
+	}
+#ifdef SHOW_ITERATIONS
+	b3Printf("Needed %d iterations to process all intersections\n", m);
+#endif
+
+	if (cmp > 0)
+	{
+		firstFaceEdge->reverse->target = faceEdge->target;
+		firstIntersection->reverse->link(firstFaceEdge);
+		firstFaceEdge->link(faceEdge->reverse);
+	}
+	else if (firstFaceEdge != faceEdge->reverse)
+	{
+		stack.push_back(faceEdge->target);
+		while (firstFaceEdge->next != faceEdge->reverse)
+		{
+			Vertex* removed = firstFaceEdge->next->target;
+			removeEdgePair(firstFaceEdge->next);
+			stack.push_back(removed);
+#ifdef DEBUG_CONVEX_HULL
+			b3Printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+		}
+		stack.push_back(NULL);
+	}
+
+	b3Assert(stack.size() > 0);
+	vertexList = stack[0];
+
+#ifdef DEBUG_CONVEX_HULL
+	b3Printf("Removing part\n");
+#endif
+#ifdef SHOW_ITERATIONS
+	n = 0;
+#endif
+	int pos = 0;
+	while (pos < stack.size())
+	{
+		int end = stack.size();
+		while (pos < end)
+		{
+			Vertex* kept = stack[pos++];
+#ifdef DEBUG_CONVEX_HULL
+			kept->print();
+#endif
+			bool deeper = false;
+			Vertex* removed;
+			while ((removed = stack[pos++]) != NULL)
+			{
+#ifdef SHOW_ITERATIONS
+				n++;
+#endif
+				kept->receiveNearbyFaces(removed);
+				while (removed->edges)
+				{
+					if (!deeper)
+					{
+						deeper = true;
+						stack.push_back(kept);
+					}
+					stack.push_back(removed->edges->target);
+					removeEdgePair(removed->edges);
+				}
+			}
+			if (deeper)
+			{
+				stack.push_back(NULL);
+			}
+		}
+	}
+#ifdef SHOW_ITERATIONS
+	b3Printf("Needed %d iterations to remove part\n", n);
+#endif
+
+	stack.resize(0);
+	face->origin = shiftedOrigin;
+
+	return true;
+}
+
+static int getVertexCopy(b3ConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<b3ConvexHullInternal::Vertex*>& vertices)
+{
+	int index = vertex->copy;
+	if (index < 0)
+	{
+		index = vertices.size();
+		vertex->copy = index;
+		vertices.push_back(vertex);
+#ifdef DEBUG_CONVEX_HULL
+		b3Printf("Vertex %d gets index *%d\n", vertex->point.index, index);
+#endif
+	}
+	return index;
+}
+
+b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+{
+	if (count <= 0)
+	{
+		vertices.clear();
+		edges.clear();
+		faces.clear();
+		return 0;
+	}
+
+	b3ConvexHullInternal hull;
+	hull.compute(coords, doubleCoords, stride, count);
+
+	b3Scalar shift = 0;
+	if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0))
+	{
+		vertices.clear();
+		edges.clear();
+		faces.clear();
+		return shift;
+	}
+
+	vertices.resize(0);
+	edges.resize(0);
+	faces.resize(0);
+
+	b3AlignedObjectArray<b3ConvexHullInternal::Vertex*> oldVertices;
+	getVertexCopy(hull.vertexList, oldVertices);
+	int copied = 0;
+	while (copied < oldVertices.size())
+	{
+		b3ConvexHullInternal::Vertex* v = oldVertices[copied];
+		vertices.push_back(hull.getCoordinates(v));
+		b3ConvexHullInternal::Edge* firstEdge = v->edges;
+		if (firstEdge)
+		{
+			int firstCopy = -1;
+			int prevCopy = -1;
+			b3ConvexHullInternal::Edge* e = firstEdge;
+			do
+			{
+				if (e->copy < 0)
+				{
+					int s = edges.size();
+					edges.push_back(Edge());
+					edges.push_back(Edge());
+					Edge* c = &edges[s];
+					Edge* r = &edges[s + 1];
+					e->copy = s;
+					e->reverse->copy = s + 1;
+					c->reverse = 1;
+					r->reverse = -1;
+					c->targetVertex = getVertexCopy(e->target, oldVertices);
+					r->targetVertex = copied;
+#ifdef DEBUG_CONVEX_HULL
+					b3Printf("      CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
+#endif
+				}
+				if (prevCopy >= 0)
+				{
+					edges[e->copy].next = prevCopy - e->copy;
+				}
+				else
+				{
+					firstCopy = e->copy;
+				}
+				prevCopy = e->copy;
+				e = e->next;
+			} while (e != firstEdge);
+			edges[firstCopy].next = prevCopy - firstCopy;
+		}
+		copied++;
+	}
+
+	for (int i = 0; i < copied; i++)
+	{
+		b3ConvexHullInternal::Vertex* v = oldVertices[i];
+		b3ConvexHullInternal::Edge* firstEdge = v->edges;
+		if (firstEdge)
+		{
+			b3ConvexHullInternal::Edge* e = firstEdge;
+			do
+			{
+				if (e->copy >= 0)
+				{
+#ifdef DEBUG_CONVEX_HULL
+					b3Printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
+#endif
+					faces.push_back(e->copy);
+					b3ConvexHullInternal::Edge* f = e;
+					do
+					{
+#ifdef DEBUG_CONVEX_HULL
+						b3Printf("   Face *%d\n", edges[f->copy].getTargetVertex());
+#endif
+						f->copy = -1;
+						f = f->reverse->prev;
+					} while (f != e);
+				}
+				e = e->next;
+			} while (e != firstEdge);
+		}
+	}
+
+	return shift;
+}

Engine/lib/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h

@@ -0,0 +1,99 @@
+/*
+Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 B3_CONVEX_HULL_COMPUTER_H
+#define B3_CONVEX_HULL_COMPUTER_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+/// Convex hull implementation based on Preparata and Hong
+/// See http://code.google.com/p/bullet/issues/detail?id=275
+/// Ole Kniemeyer, MAXON Computer GmbH
+class b3ConvexHullComputer
+{
+private:
+	b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
+
+public:
+	class Edge
+	{
+	private:
+		int next;
+		int reverse;
+		int targetVertex;
+
+		friend class b3ConvexHullComputer;
+
+	public:
+		int getSourceVertex() const
+		{
+			return (this + reverse)->targetVertex;
+		}
+
+		int getTargetVertex() const
+		{
+			return targetVertex;
+		}
+
+		const Edge* getNextEdgeOfVertex() const  // clockwise list of all edges of a vertex
+		{
+			return this + next;
+		}
+
+		const Edge* getNextEdgeOfFace() const  // counter-clockwise list of all edges of a face
+		{
+			return (this + reverse)->getNextEdgeOfVertex();
+		}
+
+		const Edge* getReverseEdge() const
+		{
+			return this + reverse;
+		}
+	};
+
+	// Vertices of the output hull
+	b3AlignedObjectArray<b3Vector3> vertices;
+
+	// Edges of the output hull
+	b3AlignedObjectArray<Edge> edges;
+
+	// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+	b3AlignedObjectArray<int> faces;
+
+	/*
+		Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
+		between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
+		by that amount (each face is moved by "shrink" length units towards the center along its normal).
+		If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
+		is the minimum distance of a face to the center of the convex hull.
+
+		The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
+		that the resulting convex hull is empty.
+
+		The output convex hull can be found in the member variables "vertices", "edges", "faces".
+		*/
+	b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+	{
+		return compute(coords, false, stride, count, shrink, shrinkClamp);
+	}
+
+	// same as above, but double precision
+	b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+	{
+		return compute(coords, true, stride, count, shrink, shrinkClamp);
+	}
+};
+
+#endif  //B3_CONVEX_HULL_COMPUTER_H

Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp

@@ -0,0 +1,174 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b3GeometryUtil.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 b3BulletMathProbe();
+
+	void b3BulletMathProbe() {}
+}
+
+bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
+{
+	int numbrushes = planeEquations.size();
+	for (int i = 0; i < numbrushes; i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+		b3Scalar dist = b3Scalar(N1.dot(point)) + b3Scalar(N1[3]) - margin;
+		if (dist > b3Scalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
+{
+	int numvertices = vertices.size();
+	for (int i = 0; i < numvertices; i++)
+	{
+		const b3Vector3& N1 = vertices[i];
+		b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin;
+		if (dist > b3Scalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations);
+
+bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations)
+{
+	int numbrushes = planeEquations.size();
+	for (int i = 0; i < numbrushes; i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+		if (planeEquation.dot(N1) > b3Scalar(0.999))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut)
+{
+	const int numvertices = vertices.size();
+	// brute force:
+	for (int i = 0; i < numvertices; i++)
+	{
+		const b3Vector3& N1 = vertices[i];
+
+		for (int j = i + 1; j < numvertices; j++)
+		{
+			const b3Vector3& N2 = vertices[j];
+
+			for (int k = j + 1; k < numvertices; k++)
+			{
+				const b3Vector3& N3 = vertices[k];
+
+				b3Vector3 planeEquation, edge0, edge1;
+				edge0 = N2 - N1;
+				edge1 = N3 - N1;
+				b3Scalar normalSign = b3Scalar(1.);
+				for (int ww = 0; ww < 2; ww++)
+				{
+					planeEquation = normalSign * edge0.cross(edge1);
+					if (planeEquation.length2() > b3Scalar(0.0001))
+					{
+						planeEquation.normalize();
+						if (notExist(planeEquation, planeEquationsOut))
+						{
+							planeEquation[3] = -planeEquation.dot(N1);
+
+							//check if inside, and replace supportingVertexOut if needed
+							if (areVerticesBehindPlane(planeEquation, vertices, b3Scalar(0.01)))
+							{
+								planeEquationsOut.push_back(planeEquation);
+							}
+						}
+					}
+					normalSign = b3Scalar(-1.);
+				}
+			}
+		}
+	}
+}
+
+void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
+{
+	const int numbrushes = planeEquations.size();
+	// brute force:
+	for (int i = 0; i < numbrushes; i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+
+		for (int j = i + 1; j < numbrushes; j++)
+		{
+			const b3Vector3& N2 = planeEquations[j];
+
+			for (int k = j + 1; k < numbrushes; k++)
+			{
+				const b3Vector3& N3 = planeEquations[k];
+
+				b3Vector3 n2n3;
+				n2n3 = N2.cross(N3);
+				b3Vector3 n3n1;
+				n3n1 = N3.cross(N1);
+				b3Vector3 n1n2;
+				n1n2 = N1.cross(N2);
+
+				if ((n2n3.length2() > b3Scalar(0.0001)) &&
+					(n3n1.length2() > b3Scalar(0.0001)) &&
+					(n1n2.length2() > b3Scalar(0.0001)))
+				{
+					//point P out of 3 plane equations:
+
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
+					//P =  -------------------------------------------------------------------------
+					//   N1 . ( N2 * N3 )
+
+					b3Scalar quotient = (N1.dot(n2n3));
+					if (b3Fabs(quotient) > b3Scalar(0.000001))
+					{
+						quotient = b3Scalar(-1.) / quotient;
+						n2n3 *= N1[3];
+						n3n1 *= N2[3];
+						n1n2 *= N3[3];
+						b3Vector3 potentialVertex = n2n3;
+						potentialVertex += n3n1;
+						potentialVertex += n1n2;
+						potentialVertex *= quotient;
+
+						//check if inside, and replace supportingVertexOut if needed
+						if (isPointInsidePlanes(planeEquations, potentialVertex, b3Scalar(0.01)))
+						{
+							verticesOut.push_back(potentialVertex);
+						}
+					}
+				}
+			}
+		}
+	}
+}

Engine/lib/bullet/src/Bullet3Geometry/b3GeometryUtil.h

@@ -0,0 +1,36 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GEOMETRY_UTIL_H
+#define B3_GEOMETRY_UTIL_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices.
+class b3GeometryUtil
+{
+public:
+	static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut);
+
+	static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut);
+
+	static bool isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar margin);
+
+	static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
+
+	static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
+};
+
+#endif  //B3_GEOMETRY_UTIL_H

Engine/lib/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h

@@ -0,0 +1,116 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2011 Advanced Micro Devices, Inc.  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 B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
+#define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+struct b3GrahamVector3 : public b3Vector3
+{
+	b3GrahamVector3(const b3Vector3& org, int orgIndex)
+		: b3Vector3(org),
+		  m_orgIndex(orgIndex)
+	{
+	}
+	b3Scalar m_angle;
+	int m_orgIndex;
+};
+
+struct b3AngleCompareFunc
+{
+	b3Vector3 m_anchor;
+	b3AngleCompareFunc(const b3Vector3& anchor)
+		: m_anchor(anchor)
+	{
+	}
+	bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const
+	{
+		if (a.m_angle != b.m_angle)
+			return a.m_angle < b.m_angle;
+		else
+		{
+			b3Scalar al = (a - m_anchor).length2();
+			b3Scalar bl = (b - m_anchor).length2();
+			if (al != bl)
+				return al < bl;
+			else
+			{
+				return a.m_orgIndex < b.m_orgIndex;
+			}
+		}
+	}
+};
+
+inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& originalPoints, b3AlignedObjectArray<b3GrahamVector3>& hull, const b3Vector3& normalAxis)
+{
+	b3Vector3 axis0, axis1;
+	b3PlaneSpace1(normalAxis, axis0, axis1);
+
+	if (originalPoints.size() <= 1)
+	{
+		for (int i = 0; i < originalPoints.size(); i++)
+			hull.push_back(originalPoints[0]);
+		return;
+	}
+	//step1 : find anchor point with smallest projection on axis0 and move it to first location
+	for (int i = 0; i < originalPoints.size(); i++)
+	{
+		//		const b3Vector3& left = originalPoints[i];
+		//		const b3Vector3& right = originalPoints[0];
+		b3Scalar projL = originalPoints[i].dot(axis0);
+		b3Scalar projR = originalPoints[0].dot(axis0);
+		if (projL < projR)
+		{
+			originalPoints.swap(0, i);
+		}
+	}
+
+	//also precompute angles
+	originalPoints[0].m_angle = -1e30f;
+	for (int i = 1; i < originalPoints.size(); i++)
+	{
+		b3Vector3 xvec = axis0;
+		b3Vector3 ar = originalPoints[i] - originalPoints[0];
+		originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length();
+	}
+
+	//step 2: sort all points, based on 'angle' with this anchor
+	b3AngleCompareFunc comp(originalPoints[0]);
+	originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
+
+	int i;
+	for (i = 0; i < 2; i++)
+		hull.push_back(originalPoints[i]);
+
+	//step 3: keep all 'convex' points and discard concave points (using back tracking)
+	for (; i != originalPoints.size(); i++)
+	{
+		bool isConvex = false;
+		while (!isConvex && hull.size() > 1)
+		{
+			b3Vector3& a = hull[hull.size() - 2];
+			b3Vector3& b = hull[hull.size() - 1];
+			isConvex = b3Cross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;
+			if (!isConvex)
+				hull.pop_back();
+			else
+				hull.push_back(originalPoints[i]);
+		}
+	}
+}
+
+#endif  //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H

Engine/lib/bullet/src/Bullet3Geometry/premake4.lua

@@ -0,0 +1,16 @@
+	project "Bullet3Geometry"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+	
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h

@@ -0,0 +1,42 @@
+
+#ifndef B3_GPU_BROADPHASE_INTERFACE_H
+#define B3_GPU_BROADPHASE_INTERFACE_H
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "b3SapAabb.h"
+#include "Bullet3Common/shared/b3Int2.h"
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
+
+class b3GpuBroadphaseInterface
+{
+public:
+	typedef class b3GpuBroadphaseInterface*(CreateFunc)(cl_context ctx, cl_device_id device, cl_command_queue q);
+
+	virtual ~b3GpuBroadphaseInterface()
+	{
+	}
+
+	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
+	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
+
+	virtual void calculateOverlappingPairs(int maxPairs) = 0;
+	virtual void calculateOverlappingPairsHost(int maxPairs) = 0;
+
+	//call writeAabbsToGpu after done making all changes (createProxy etc)
+	virtual void writeAabbsToGpu() = 0;
+
+	virtual cl_mem getAabbBufferWS() = 0;
+	virtual int getNumOverlap() = 0;
+	virtual cl_mem getOverlappingPairBuffer() = 0;
+
+	virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU() = 0;
+	virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU() = 0;
+
+	virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() = 0;
+	virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() = 0;
+	virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() = 0;
+};
+
+#endif  //B3_GPU_BROADPHASE_INTERFACE_H

Engine/lib/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp

@@ -0,0 +1,338 @@
+
+#include "b3GpuGridBroadphase.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+#include "kernels/gridBroadphaseKernels.h"
+#include "kernels/sapKernels.h"
+//#include "kernels/gridBroadphase.cl"
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
+
+#define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
+#define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl"
+
+cl_kernel kCalcHashAABB;
+cl_kernel kClearCellStart;
+cl_kernel kFindCellStart;
+cl_kernel kFindOverlappingPairs;
+cl_kernel m_copyAabbsKernel;
+cl_kernel m_sap2Kernel;
+
+//int maxPairsPerBody = 64;
+int maxBodiesPerCell = 256;  //??
+
+b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q)
+	: m_context(ctx),
+	  m_device(device),
+	  m_queue(q),
+	  m_allAabbsGPU1(ctx, q),
+	  m_smallAabbsMappingGPU(ctx, q),
+	  m_largeAabbsMappingGPU(ctx, q),
+	  m_gpuPairs(ctx, q),
+
+	  m_hashGpu(ctx, q),
+
+	  m_cellStartGpu(ctx, q),
+	  m_paramsGPU(ctx, q)
+{
+	b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
+	b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
+
+	m_paramsCPU.m_gridSize[0] = 128;
+	m_paramsCPU.m_gridSize[1] = 128;
+	m_paramsCPU.m_gridSize[2] = 128;
+	m_paramsCPU.m_gridSize[3] = maxBodiesPerCell;
+	m_paramsCPU.setMaxBodiesPerCell(maxBodiesPerCell);
+	m_paramsCPU.m_invCellSize[0] = invGridSize[0];
+	m_paramsCPU.m_invCellSize[1] = invGridSize[1];
+	m_paramsCPU.m_invCellSize[2] = invGridSize[2];
+	m_paramsCPU.m_invCellSize[3] = 0.f;
+	m_paramsGPU.push_back(m_paramsCPU);
+
+	cl_int errNum = 0;
+
+	{
+		const char* sapSrc = sapCL;
+		cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
+		b3Assert(errNum == CL_SUCCESS);
+		m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
+		m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
+		b3Assert(errNum == CL_SUCCESS);
+	}
+
+	{
+		cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kCalcHashAABB", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kClearCellStart", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+	}
+
+	m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
+}
+b3GpuGridBroadphase::~b3GpuGridBroadphase()
+{
+	clReleaseKernel(kCalcHashAABB);
+	clReleaseKernel(kClearCellStart);
+	clReleaseKernel(kFindCellStart);
+	clReleaseKernel(kFindOverlappingPairs);
+	clReleaseKernel(m_sap2Kernel);
+	clReleaseKernel(m_copyAabbsKernel);
+
+	delete m_sorter;
+}
+
+void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
+{
+	b3SapAabb aabb;
+	aabb.m_minVec = aabbMin;
+	aabb.m_maxVec = aabbMax;
+	aabb.m_minIndices[3] = userPtr;
+	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
+	m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
+
+	m_allAabbsCPU1.push_back(aabb);
+}
+void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
+{
+	b3SapAabb aabb;
+	aabb.m_minVec = aabbMin;
+	aabb.m_maxVec = aabbMax;
+	aabb.m_minIndices[3] = userPtr;
+	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
+	m_largeAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
+
+	m_allAabbsCPU1.push_back(aabb);
+}
+
+void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
+{
+	B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
+
+	if (0)
+	{
+		calculateOverlappingPairsHost(maxPairs);
+		/*
+		b3AlignedObjectArray<b3Int4> cpuPairs;
+		m_gpuPairs.copyToHost(cpuPairs);
+		printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
+		for (int i=0;i<m_gpuPairs.size();i++)
+		{
+			printf("host pair %d = %d,%d\n",i,cpuPairs[i].x,cpuPairs[i].y);
+		}
+		*/
+		return;
+	}
+
+	int numSmallAabbs = m_smallAabbsMappingGPU.size();
+
+	b3OpenCLArray<int> pairCount(m_context, m_queue);
+	pairCount.push_back(0);
+	m_gpuPairs.resize(maxPairs);  //numSmallAabbs*maxPairsPerBody);
+
+	{
+		int numLargeAabbs = m_largeAabbsMappingGPU.size();
+		if (numLargeAabbs && numSmallAabbs)
+		{
+			B3_PROFILE("sap2Kernel");
+			b3BufferInfoCL bInfo[] = {
+				b3BufferInfoCL(m_allAabbsGPU1.getBufferCL()),
+				b3BufferInfoCL(m_largeAabbsMappingGPU.getBufferCL()),
+				b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL()),
+				b3BufferInfoCL(m_gpuPairs.getBufferCL()),
+				b3BufferInfoCL(pairCount.getBufferCL())};
+			b3LauncherCL launcher(m_queue, m_sap2Kernel, "m_sap2Kernel");
+			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
+			launcher.setConst(numLargeAabbs);
+			launcher.setConst(numSmallAabbs);
+			launcher.setConst(0);  //axis is not used
+			launcher.setConst(maxPairs);
+			//@todo: use actual maximum work item sizes of the device instead of hardcoded values
+			launcher.launch2D(numLargeAabbs, numSmallAabbs, 4, 64);
+
+			int numPairs = pairCount.at(0);
+
+			if (numPairs > maxPairs)
+			{
+				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
+				numPairs = maxPairs;
+			}
+		}
+	}
+
+	if (numSmallAabbs)
+	{
+		B3_PROFILE("gridKernel");
+		m_hashGpu.resize(numSmallAabbs);
+		{
+			B3_PROFILE("kCalcHashAABB");
+			b3LauncherCL launch(m_queue, kCalcHashAABB, "kCalcHashAABB");
+			launch.setConst(numSmallAabbs);
+			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
+			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
+			launch.setBuffer(m_hashGpu.getBufferCL());
+			launch.setBuffer(this->m_paramsGPU.getBufferCL());
+			launch.launch1D(numSmallAabbs);
+		}
+
+		m_sorter->execute(m_hashGpu);
+
+		int numCells = this->m_paramsCPU.m_gridSize[0] * this->m_paramsCPU.m_gridSize[1] * this->m_paramsCPU.m_gridSize[2];
+		m_cellStartGpu.resize(numCells);
+		//b3AlignedObjectArray<int >			cellStartCpu;
+
+		{
+			B3_PROFILE("kClearCellStart");
+			b3LauncherCL launch(m_queue, kClearCellStart, "kClearCellStart");
+			launch.setConst(numCells);
+			launch.setBuffer(m_cellStartGpu.getBufferCL());
+			launch.launch1D(numCells);
+			//m_cellStartGpu.copyToHost(cellStartCpu);
+			//printf("??\n");
+		}
+
+		{
+			B3_PROFILE("kFindCellStart");
+			b3LauncherCL launch(m_queue, kFindCellStart, "kFindCellStart");
+			launch.setConst(numSmallAabbs);
+			launch.setBuffer(m_hashGpu.getBufferCL());
+			launch.setBuffer(m_cellStartGpu.getBufferCL());
+			launch.launch1D(numSmallAabbs);
+			//m_cellStartGpu.copyToHost(cellStartCpu);
+			//printf("??\n");
+		}
+
+		{
+			B3_PROFILE("kFindOverlappingPairs");
+
+			b3LauncherCL launch(m_queue, kFindOverlappingPairs, "kFindOverlappingPairs");
+			launch.setConst(numSmallAabbs);
+			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
+			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
+			launch.setBuffer(m_hashGpu.getBufferCL());
+			launch.setBuffer(m_cellStartGpu.getBufferCL());
+
+			launch.setBuffer(m_paramsGPU.getBufferCL());
+			//launch.setBuffer(0);
+			launch.setBuffer(pairCount.getBufferCL());
+			launch.setBuffer(m_gpuPairs.getBufferCL());
+
+			launch.setConst(maxPairs);
+			launch.launch1D(numSmallAabbs);
+
+			int numPairs = pairCount.at(0);
+			if (numPairs > maxPairs)
+			{
+				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
+				numPairs = maxPairs;
+			}
+
+			m_gpuPairs.resize(numPairs);
+
+			if (0)
+			{
+				b3AlignedObjectArray<b3Int4> pairsCpu;
+				m_gpuPairs.copyToHost(pairsCpu);
+
+				int sz = m_gpuPairs.size();
+				printf("m_gpuPairs.size()=%d\n", sz);
+				for (int i = 0; i < m_gpuPairs.size(); i++)
+				{
+					printf("pair %d = %d,%d\n", i, pairsCpu[i].x, pairsCpu[i].y);
+				}
+
+				printf("?!?\n");
+			}
+		}
+	}
+
+	//calculateOverlappingPairsHost(maxPairs);
+}
+void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
+{
+	m_hostPairs.resize(0);
+	m_allAabbsGPU1.copyToHost(m_allAabbsCPU1);
+	for (int i = 0; i < m_allAabbsCPU1.size(); i++)
+	{
+		for (int j = i + 1; j < m_allAabbsCPU1.size(); j++)
+		{
+			if (b3TestAabbAgainstAabb2(m_allAabbsCPU1[i].m_minVec, m_allAabbsCPU1[i].m_maxVec,
+									   m_allAabbsCPU1[j].m_minVec, m_allAabbsCPU1[j].m_maxVec))
+			{
+				b3Int4 pair;
+				int a = m_allAabbsCPU1[j].m_minIndices[3];
+				int b = m_allAabbsCPU1[i].m_minIndices[3];
+				if (a <= b)
+				{
+					pair.x = a;
+					pair.y = b;  //store the original index in the unsorted aabb array
+				}
+				else
+				{
+					pair.x = b;
+					pair.y = a;  //store the original index in the unsorted aabb array
+				}
+
+				if (m_hostPairs.size() < maxPairs)
+				{
+					m_hostPairs.push_back(pair);
+				}
+			}
+		}
+	}
+
+	m_gpuPairs.copyFromHost(m_hostPairs);
+}
+
+//call writeAabbsToGpu after done making all changes (createProxy etc)
+void b3GpuGridBroadphase::writeAabbsToGpu()
+{
+	m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1);
+	m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
+	m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
+}
+
+cl_mem b3GpuGridBroadphase::getAabbBufferWS()
+{
+	return this->m_allAabbsGPU1.getBufferCL();
+}
+int b3GpuGridBroadphase::getNumOverlap()
+{
+	return m_gpuPairs.size();
+}
+cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
+{
+	return m_gpuPairs.getBufferCL();
+}
+
+b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
+{
+	return m_allAabbsGPU1;
+}
+
+b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
+{
+	return m_allAabbsCPU1;
+}
+
+b3OpenCLArray<b3Int4>& b3GpuGridBroadphase::getOverlappingPairsGPU()
+{
+	return m_gpuPairs;
+}
+b3OpenCLArray<int>& b3GpuGridBroadphase::getSmallAabbIndicesGPU()
+{
+	return m_smallAabbsMappingGPU;
+}
+b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
+{
+	return m_largeAabbsMappingGPU;
+}