Update Bullet to 2.83.6. Thanks to Jukka for the initial work & Emscripten fix. Closes #929.

Docs/Urho3D.dox

@@ -140,7 +140,7 @@ Urho3D uses the following third-party libraries:
 
 - AngelScript 2.30.2 (http://www.angelcode.com/angelscript/)
 - Box2D 2.3.0 (http://box2d.org/)
-- Bullet 2.82 (http://www.bulletphysics.org/)
+- Bullet 2.83.6 (http://www.bulletphysics.org/)
 - Civetweb (http://sourceforge.net/projects/civetweb/)
 - FreeType 2.5.0 (http://www.freetype.org/)
 - GLEW 1.9.0 (http://glew.sourceforge.net/)

README.md

@@ -101,7 +101,7 @@ Urho3D is greatly inspired by OGRE (http://www.ogre3d.org) and Horde3D
 Urho3D uses the following third-party libraries:
 - AngelScript 2.30.2 (http://www.angelcode.com/angelscript/)
 - Box2D 2.3.0 (http://box2d.org/)
-- Bullet 2.82 (http://www.bulletphysics.org/)
+- Bullet 2.83.6 (http://www.bulletphysics.org/)
 - Civetweb (http://sourceforge.net/projects/civetweb/)
 - FreeType 2.5.0 (http://www.freetype.org/)
 - GLEW 1.9.0 (http://glew.sourceforge.net/)

Source/ThirdParty/Bullet/AUTHORS

@@ -1,22 +0,0 @@
-
-Bullet Physics Library is an open source project with help from the community at the Physics Forum
-See the forum at http://bulletphysics.com
-
-The project was started by Erwin Coumans
-
-Following people contributed to Bullet
-(random order, please let us know on the forum if your name should be in this list)
-
-Gino van den Bergen: LinearMath classes
-Christer Ericson: parts of the voronoi simplex solver
-Simon Hobbs: 3d axis sweep and prune, Extras/SATCollision, separating axis theorem + SIMD code
-Dirk Gregorius: generic D6 constraint
-Erin Catto: accumulated impulse in sequential impulse
-Nathanael Presson: EPA penetration depth calculation
-Francisco Leon: GIMPACT Concave Concave collision
-Joerg Henrichs: make buildsystem (work in progress)
-Eric Sunshine: jam + msvcgen buildsystem
-Steve Baker: GPU physics and general implementation improvements
-Jay Lee: Double precision support
-KleMiX, aka Vsevolod Klementjev, managed version, rewritten in C# for XNA
-Erwin Coumans: most other source code

Source/ThirdParty/Bullet/AUTHORS.txt

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

Source/ThirdParty/Bullet/CMakeLists.txt

@@ -28,15 +28,15 @@ file (GLOB CPP_FILES src/BulletCollision/BroadphaseCollision/*.cpp
     src/BulletCollision/CollisionDispatch/*.cpp src/BulletCollision/CollisionShapes/*.cpp 
     src/BulletCollision/Gimpact/*.cpp src/BulletCollision/NarrowPhaseCollision/*.cpp 
     src/BulletDynamics/Character/*.cpp src/BulletDynamics/ConstraintSolver/*.cpp
-    src/BulletDynamics/Dynamics/*.cpp src/BulletDynamics/Vehicle/*.cpp src/BulletSoftBody/*.cpp
-    src/BulletDynamics/Featherstone/*.cpp src/BulletDynamics/MLCPSolvers/*.cpp
+    src/BulletDynamics/Dynamics/*.cpp src/BulletDynamics/Featherstone/*.cpp 
+    src/BulletDynamics/MLCPSolvers/*.cpp src/BulletDynamics/Vehicle/*.cpp src/BulletSoftBody/*.cpp
     src/LinearMath/*.cpp)
 file (GLOB H_FILES *.h src/BulletCollision/BroadphaseCollision/*.h
     src/BulletCollision/CollisionDispatch/*.h src/BulletCollision/CollisionShapes/*.h
     src/BulletCollision/Gimpact/*.h src/BulletCollision/NarrowPhaseCollision/*.h
     src/BulletDynamics/Character/*.h src/BulletDynamics/ConstraintSolver/*.h
-    src/BulletDynamics/Dynamics/*.h src/BulletDynamics/Vehicle/*.h src/BulletSoftBody/*.h 
-    src/BulletDynamics/Featherstone/*.h src/BulletDynamics/MLCPSolvers/*.h
+    src/BulletDynamics/Dynamics/*.h src/BulletDynamics/Featherstone/*.h 
+    src/BulletDynamics/MLCPSolvers/*.h src/BulletDynamics/Vehicle/*.h src/BulletSoftBody/*.h
     src/LinearMath/*.h)
 set (SOURCE_FILES ${CPP_FILES} ${H_FILES})
 

Source/ThirdParty/Bullet/INSTALL

@@ -1,111 +0,0 @@
-Bullet Collision Detection and Physics Library
-
-See also http://bulletphysics.org/mediawiki-1.5.8/index.php/Creating_a_project_from_scratch
-
-** Windows Compilation **
-
-	Open the Microsoft Visual Studio solution in msvc/20xx/BULLET_PHYSICS.sln
-
-Alternatively, use CMake to autogenerate a build system for Windows:
-	
-	- Download/install CMake from www.cmake.org or package manager
-	- Use cmake-gui or
-	- List available build systems by running 'cmake' in the Bullet root folder
-	- Use cmake-gui 
-	- Create a build system using the -G option for example:
-	
-	cmake . -G "Visual Studio 9 2008" or
-	cmake . -G "Visual Studio 9 2008 Win64"
-
-
-** Linux Compilation **
-
-  - Download/install CMake from www.cmake.org or package manager
-    CMake is like autoconf in that it will create build scripts which are then 
-    used for the actual compilation
-
-	- List available build systems by running 'cmake' in the Bullet root folder
-	- Create a build system using the -G option for example:
-
-	cmake . -G "Unix Makefiles"
-
-  - There are some options for cmake builds:
-      BUILD_SHARED_LIBS: default 'OFF', set to 'ON' to build .so libraries
-      BUILD_EXTRAS: default 'ON', compiles additional libraries in 'Extras'
-      BUILD_DEMOS: default 'ON', compiles applications found in 'Demos'
-      CMAKE_INSTALL_PREFIX: default '/usr/local', the installation path.
-      CMAKE_INSTALL_RPATH: if you install outside a standard ld search path,
-        then you should set this to the installation lib path.
-      CMAKE_BUILD_TYPE: default 'Release', can include debug symbols with
-        either 'Debug' or 'RelWithDebInfo'.
-    Other options may be discovered by 'cmake --help-variable-list' and
-    'cmake --help-variable OPTION'
-
-  - Run 'cmake' with desired options of the form -DOPTION=VALUE
-    By default this will create the usual Makefile build system, but CMake can 
-    also produce Eclipse or KDevelop project files.  See 'cmake --help' to see 
-    what "generators" are available in your environment, selected via '-G'.
-        For example:
-        cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=RelWithDebugInfo
-
-  - Assuming using the default Makefile output from cmake, run 'make' to 
-    build, and then 'make install' if you wish to install.
-
-
-** Mac OS X Compilation **
-
-  - Download/install CMake from www.cmake.org or package manager
-    CMake is like autoconf in that it will create build scripts which are then 
-    used for the actual compilation
-
-	- List available build systems by running 'cmake' in the Bullet root folder
-	- Create a build system using the -G option for example:
-
-  cmake . -G Xcode
-  cmake . -G "Unix Makefiles"
-
-  - There are some options for cmake builds:
-      BUILD_SHARED_LIBS: default 'OFF', set to 'ON' to build .dylib libraries
-      BUILD_EXTRAS: default 'ON', compiles additional libraries in 'Extras'
-      BUILD_DEMOS: default 'ON', compiles applications found in 'Demos'
-      CMAKE_INSTALL_PREFIX: default '/usr/local', the installation path.
-      CMAKE_INSTALL_NAME_DIR: if you install outside a standard ld search 
-        path, then you should set this to the installation lib/framework path. 
-      CMAKE_OSX_ARCHITECTURES: defaults to the native architecture, but can be
-        set to a semicolon separated list for fat binaries, e.g. ppc;i386;x86_64
-      CMAKE_BUILD_TYPE: default 'Release', can include debug symbols with
-        either 'Debug' or 'RelWithDebInfo'.
-
-    To build framework bundles:
-      FRAMEWORK: default 'OFF', also requires 'BUILD_SHARED_LIBS' set ON
-        If both FRAMEWORK and BUILD_SHARED_LIBS are set, will create
-        OS X style Framework Bundles which can be placed in 
-        linked via the -framework gcc argument or drag into Xcode projects.
-    (If not framework, then UNIX style 'include' and 'lib' will be produced)
-      
-    Other options may be discovered by 'cmake --help-variable-list' and
-    'cmake --help-variable OPTION'
-
-  - Run 'cmake' with desired options of the form -DOPTION=VALUE
-    By default this will create the usual Makefile build system, but CMake can 
-    also produce Eclipse or KDevelop project files.  See 'cmake --help' to see 
-    what "generators" are available in your environment, selected via '-G'.
-        For example:
-        cmake -DBUILD_SHARED_LIBS=ON -DFRAMEWORK=ON \
-              -DCMAKE_INSTALL_PREFIX=/Library/Frameworks \
-              -DCMAKE_INSTALL_NAME_DIR=/Library/Frameworks \
-              -DCMAKE_OSX_ARCHITECTURES='ppc;i386;x86_64' \
-              -DCMAKE_BUILD_TYPE=RelWithDebugInfo
-
-  - Assuming using the default Makefile output from cmake, run 'make' to build 
-    and then 'make install'.
-
-
-** Alternative Mac OS X and Linux via autoconf/make **
-  - at the command line:
-    ./autogen.sh
-    ./configure
-    make
-
-
-** For more help, visit http://www.bulletphysics.org **

Source/ThirdParty/Bullet/COPYING → Source/ThirdParty/Bullet/LICENSE.txt

@@ -1,6 +1,8 @@
-/*
+
+The files in this repository are licensed under the zlib license, except for the files under 'Extras' and examples/ThirdPartyLibs.
+
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2011 Erwin Coumans  http://bulletphysics.org
+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.
@@ -11,7 +13,3 @@ subject to the following restrictions:
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 3. This notice may not be removed or altered from any source distribution.
-*/
-
-All files in the Bullet/src folder are under this Zlib license.
-Files in the Extras and Demos folder may have a different license, see the respective files.

Source/ThirdParty/Bullet/NEWS

@@ -1,5 +0,0 @@
-
-For news, visit the Bullet Physics forums at
-http://www.bulletphysics.org and http://bullet.googlecode.com
-
-

Source/ThirdParty/Bullet/README

@@ -1,6 +0,0 @@
-
-Bullet is a 3D Collision Detection and Rigid Body Dynamics Library for games and animation.
-Free for commercial use, including Playstation 3, open source under the ZLib License.
-
-See the Bullet_User_Manual.pdf for more info and visit the Bullet Physics Forum at
-http://bulletphysics.org

Source/ThirdParty/Bullet/README.md

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

Source/ThirdParty/Bullet/VERSION

@@ -1 +1 @@
-2.82
+2.84

Source/ThirdParty/Bullet/src/Bullet-C-Api.h

@@ -1,176 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
-	Work in progress, functionality will be added on demand.
-
-	If possible, use the richer Bullet C++ API, by including "btBulletDynamicsCommon.h"
-*/
-
-#ifndef BULLET_C_API_H
-#define BULLET_C_API_H
-
-#define PL_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
-
-#ifdef BT_USE_DOUBLE_PRECISION
-typedef double	plReal;
-#else
-typedef float	plReal;
-#endif
-
-typedef plReal	plVector3[3];
-typedef plReal	plQuaternion[4];
-
-#ifdef __cplusplus
-extern "C" { 
-#endif
-
-/**	Particular physics SDK (C-API) */
-	PL_DECLARE_HANDLE(plPhysicsSdkHandle);
-
-/** 	Dynamics world, belonging to some physics SDK (C-API)*/
-	PL_DECLARE_HANDLE(plDynamicsWorldHandle);
-
-/** Rigid Body that can be part of a Dynamics World (C-API)*/	
-	PL_DECLARE_HANDLE(plRigidBodyHandle);
-
-/** 	Collision Shape/Geometry, property of a Rigid Body (C-API)*/
-	PL_DECLARE_HANDLE(plCollisionShapeHandle);
-
-/** Constraint for Rigid Bodies (C-API)*/
-	PL_DECLARE_HANDLE(plConstraintHandle);
-
-/** Triangle Mesh interface (C-API)*/
-	PL_DECLARE_HANDLE(plMeshInterfaceHandle);
-
-/** Broadphase Scene/Proxy Handles (C-API)*/
-	PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
-	PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
-	PL_DECLARE_HANDLE(plCollisionWorldHandle);
-
-/**
-	Create and Delete a Physics SDK	
-*/
-
-	extern	plPhysicsSdkHandle	plNewBulletSdk(void); //this could be also another sdk, like ODE, PhysX etc.
-	extern	void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk);
-
-/** Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
-
-	typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);
-
-	extern plCollisionBroadphaseHandle	plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback);
-
-	extern void	plDestroyBroadphase(plCollisionBroadphaseHandle bp);
-
-	extern 	plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
-
-	extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle);
-
-	extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
-
-/* todo: add pair cache support with queries like add/remove/find pair */
-	
-	extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk);
-
-/* todo: add/remove objects */
-	
-
-/* Dynamics World */
-
-	extern  plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk);
-
-	extern  void           plDeleteDynamicsWorld(plDynamicsWorldHandle world);
-
-	extern	void	plStepSimulation(plDynamicsWorldHandle,	plReal	timeStep);
-
-	extern  void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
-
-	extern  void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
-
-
-/* Rigid Body  */
-
-	extern  plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape );
-
-	extern  void plDeleteRigidBody(plRigidBodyHandle body);
-
-
-/* Collision Shape definition */
-
-	extern  plCollisionShapeHandle plNewSphereShape(plReal radius);
-	extern  plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z);
-	extern  plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height);	
-	extern  plCollisionShapeHandle plNewConeShape(plReal radius, plReal height);
-	extern  plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height);
-	extern	plCollisionShapeHandle plNewCompoundShape(void);
-	extern	void	plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn);
-
-	extern  void plDeleteShape(plCollisionShapeHandle shape);
-
-	/* Convex Meshes */
-	extern  plCollisionShapeHandle plNewConvexHullShape(void);
-	extern  void		plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z);
-/* Concave static triangle meshes */
-	extern  plMeshInterfaceHandle		   plNewMeshInterface(void);
-	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
-	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
-
-	extern  void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling);
-
-/* SOLID has Response Callback/Table/Management */
-/* PhysX has Triggers, User Callbacks and filtering */
-/* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */
-
-/*	typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle	rbHandle, plVector3 pos); */
-/*	typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle	rbHandle, plQuaternion orientation); */
-
-	/* get world transform */
-	extern void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
-	extern void	plGetPosition(plRigidBodyHandle object,plVector3 position);
-	extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation);
-
-	/* set world transform (position/orientation) */
-	extern  void plSetPosition(plRigidBodyHandle object, const plVector3 position);
-	extern  void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
-	extern	void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
-	extern	void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
-
-	typedef struct plRayCastResult {
-		plRigidBodyHandle		m_body;  
-		plCollisionShapeHandle	m_shape; 		
-		plVector3				m_positionWorld; 		
-		plVector3				m_normalWorld;
-	} plRayCastResult;
-
-	extern  int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res);
-
-	/* Sweep API */
-
-	/* extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */
-
-	/* Continuous Collision Detection API */
-	
-	// needed for source/blender/blenkernel/intern/collision.c
-	double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]);
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif //BULLET_C_API_H
-

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -38,8 +38,9 @@ static DBVT_INLINE btDbvtVolume	merge(	const btDbvtVolume& a,
 									  const btDbvtVolume& b)
 {
 #if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
-	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
-	btDbvtVolume&	res=*(btDbvtVolume*)locals;
+	ATTRIBUTE_ALIGNED16( char locals[sizeof(btDbvtAabbMm)]);
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	res=*ptr;
 #else
 		btDbvtVolume	res;
 #endif
@@ -250,7 +251,8 @@ static btDbvtVolume				bounds(	const tNodeArray& leaves)
 {
 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
 	ATTRIBUTE_ALIGNED16(char	locals[sizeof(btDbvtVolume)]);
-	btDbvtVolume&	volume=*(btDbvtVolume*)locals;
+	btDbvtVolume* ptr = (btDbvtVolume*) locals;
+	btDbvtVolume&	volume=*ptr;
 	volume=leaves[0]->volume;
 #else
 	btDbvtVolume volume=leaves[0]->volume;

Source/ThirdParty/Bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -1193,20 +1193,26 @@ inline void		btDbvt::collideOCL(	const btDbvtNode* root,
 							/* Insert 0	*/ 
 							j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
 							stack.push_back(0);
-#if DBVT_USE_MEMMOVE
-							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
-#else
-							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
-#endif
+							
+							//void * memmove ( void * destination, const void * source, size_t num );
+							
+//#if DBVT_USE_MEMMOVE
+//							memmove(&stack[j],&stack[j-1],sizeof(int)*(stack.size()-j-1));
+//#else
+							for(int k=stack.size()-1;k>j;--k) 
+							{
+								stack[k]=stack[k-1];
+							}
+//#endif
 							stack[j]=allocate(ifree,stock,nes[q]);
 							/* Insert 1	*/ 
 							j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
 							stack.push_back(0);
-#if DBVT_USE_MEMMOVE
-							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
-#else
+//#if DBVT_USE_MEMMOVE
+//							memmove(&stack[j],&stack[j-1],sizeof(int)*(stack.size()-j-1));
+//#else
 							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
-#endif
+//#endif
 							stack[j]=allocate(ifree,stock,nes[1-q]);
 						}
 						else

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -189,7 +189,7 @@ bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
 
 	if ((!body0->isActive()) && (!body1->isActive()))
 		needsCollision = false;
-	else if (!body0->checkCollideWith(body1))
+	else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
 		needsCollision = false;
 	
 	return needsCollision ;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -31,10 +31,11 @@ btCollisionObject::btCollisionObject()
 		m_activationState1(1),
 		m_deactivationTime(btScalar(0.)),
 		m_friction(btScalar(0.5)),
-		m_rollingFriction(0.0f),
 		m_restitution(btScalar(0.)),
+		m_rollingFriction(0.0f),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+		m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
 		m_ccdMotionThreshold(btScalar(0.)),

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -92,11 +92,10 @@ protected:
 	int				m_internalType;
 
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-	union
-	{
-		void*			m_userObjectPointer;
-		int	m_userIndex;
-	};
+
+    void*			m_userObjectPointer;
+    
+    int	m_userIndex;
 
 	///time of impact calculation
 	btScalar		m_hitFraction; 
@@ -110,13 +109,11 @@ protected:
 	/// If some object should have elaborate collision filtering by sub-classes
 	int			m_checkCollideWith;
 
+	btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;
+
 	///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
 	int			m_updateRevision;
 
-	virtual bool	checkCollideWithOverride(const btCollisionObject* /* co */) const
-	{
-		return true;
-	}
 
 public:
 
@@ -225,7 +222,34 @@ public:
 		return m_collisionShape;
 	}
 
-	
+	void	setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
+	{
+		if (ignoreCollisionCheck)
+		{
+			//We don't check for duplicates. Is it ok to leave that up to the user of this API?
+			//int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+			//if (index == m_objectsWithoutCollisionCheck.size())
+			//{
+			m_objectsWithoutCollisionCheck.push_back(co);
+			//}
+		}
+		else
+		{
+			m_objectsWithoutCollisionCheck.remove(co);
+		}
+		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
+	}
+
+	virtual bool	checkCollideWithOverride(const btCollisionObject*  co) const
+	{
+		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
+		if (index < m_objectsWithoutCollisionCheck.size())
+		{
+			return false;
+		}
+		return true;
+	}
+
 
 	
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h

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

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -36,7 +36,7 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
-#include "BulletCollision/Gimpact/btGImpactShape.h"
+
 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
 
@@ -294,12 +294,13 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 		btGjkConvexCast	gjkConvexCaster(castShape,convexShape,&simplexSolver);
 		
 		//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
-		bool condition = true;
+
 		btConvexCast* convexCasterPtr = 0;
-		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
-			convexCasterPtr = &subSimplexConvexCaster;
-		else
+		//use kF_UseSubSimplexConvexCastRaytest by default
+		if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest)
 			convexCasterPtr = &gjkConvexCaster;
+		else
+			convexCasterPtr = &subSimplexConvexCaster;
 		
 		btConvexCast& convexCaster = *convexCasterPtr;
 
@@ -310,6 +311,7 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 			{
 				if (castResult.m_fraction < resultCallback.m_closestHitFraction)
 				{
+					//todo: figure out what this is about. When is rayFromTest.getBasis() not identity?
 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
 					//rotate normal into worldspace
 					castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal;
@@ -389,14 +391,7 @@ void	btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
 			}
-			else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
-			{
-				btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape;
-
-				BridgeTriangleRaycastCallback	rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
-				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
-				concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal);
-			}else
+			else
 			{
 				//generic (slower) case
 				btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
@@ -772,7 +767,7 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 									hitPointLocal,
 									hitFraction);
 
-								bool	normalInWorldSpace = false;
+								bool	normalInWorldSpace = true;
 
 								return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
 							}
@@ -1261,8 +1256,11 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 		return;
 	
 	// Draw a small simplex at the center of the object
-	getDebugDrawer()->drawTransform(worldTransform,1);
-	
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
+	{
+		getDebugDrawer()->drawTransform(worldTransform,1);
+	}
+
 	// Urho3D: never draw heightfields as they are potentially huge
 	if (shape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
 		return;
@@ -1443,81 +1441,91 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 
 void	btCollisionWorld::debugDrawWorld()
 {
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	if (getDebugDrawer())
 	{
-		int numManifolds = getDispatcher()->getNumManifolds();
-		btVector3 color(1,1,0);
-		for (int i=0;i<numManifolds;i++)
+		btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors();
+
+		if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
 		{
-			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
-			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
-			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+		
 
-			int numContacts = contactManifold->getNumContacts();
-			for (int j=0;j<numContacts;j++)
+			if (getDispatcher())
 			{
-				btManifoldPoint& cp = contactManifold->getContactPoint(j);
-				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+				int numManifolds = getDispatcher()->getNumManifolds();
+			
+				for (int i=0;i<numManifolds;i++)
+				{
+					btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+					//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+					//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+					int numContacts = contactManifold->getNumContacts();
+					for (int j=0;j<numContacts;j++)
+					{
+						btManifoldPoint& cp = contactManifold->getContactPoint(j);
+						getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint);
+					}
+				}
 			}
 		}
-	}
-
-	if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
-	{
-		int i;
 
-		for (  i=0;i<m_collisionObjects.size();i++)
+		if ((getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)))
 		{
-			btCollisionObject* colObj = m_collisionObjects[i];
-			if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
+			int i;
+
+			for (  i=0;i<m_collisionObjects.size();i++)
 			{
-				if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
+				btCollisionObject* colObj = m_collisionObjects[i];
+				if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
 				{
-					btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
-					switch(colObj->getActivationState())
+					if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe))
 					{
-					case  ACTIVE_TAG:
-						color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
-					case ISLAND_SLEEPING:
-						color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
-					case WANTS_DEACTIVATION:
-						color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
-					case DISABLE_DEACTIVATION:
-						color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
-					case DISABLE_SIMULATION:
-						color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
-					default:
+						btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4));
+
+						switch(colObj->getActivationState())
 						{
-							color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
-						}
-					};
+						case  ACTIVE_TAG:
+							color = defaultColors.m_activeObject; break;
+						case ISLAND_SLEEPING:
+							color =  defaultColors.m_deactivatedObject;break;
+						case WANTS_DEACTIVATION:
+							color = defaultColors.m_wantsDeactivationObject;break;
+						case DISABLE_DEACTIVATION:
+							color = defaultColors.m_disabledDeactivationObject;break;
+						case DISABLE_SIMULATION:
+							color = defaultColors.m_disabledSimulationObject;break;
+						default:
+							{
+								color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3));
+							}
+						};
 
-					debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
-				}
-				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-				{
-					btVector3 minAabb,maxAabb;
-					btVector3 colorvec(1,0,0);
-					colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-					btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
-					minAabb -= contactThreshold;
-					maxAabb += contactThreshold;
+						debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+					}
+					if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+					{
+						btVector3 minAabb,maxAabb;
+						btVector3 colorvec = defaultColors.m_aabb;
+						colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+						btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+						minAabb -= contactThreshold;
+						maxAabb += contactThreshold;
 
-					btVector3 minAabb2,maxAabb2;
+						btVector3 minAabb2,maxAabb2;
 
-					if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
-					{
-						colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
-						minAabb2 -= contactThreshold;
-						maxAabb2 += contactThreshold;
-						minAabb.setMin(minAabb2);
-						maxAabb.setMax(maxAabb2);
-					}
+						if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject())
+						{
+							colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
+							minAabb2 -= contactThreshold;
+							maxAabb2 += contactThreshold;
+							minAabb.setMin(minAabb2);
+							maxAabb.setMax(maxAabb2);
+						}
 
-					m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+						m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+					}
 				}
 			}
-
 		}
 	}
 }
@@ -1526,15 +1534,6 @@ void	btCollisionWorld::debugDrawWorld()
 void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 {
 	int i;
-	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
-		{
-			colObj->serializeSingleObject(serializer);
-		}
-	}
 
 	///keep track of shapes already serialized
 	btHashMap<btHashPtr,btCollisionShape*>	serializedShapes;
@@ -1551,6 +1550,15 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 		}
 	}
 
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK))
+		{
+			colObj->serializeSingleObject(serializer);
+		}
+	}
 }
 
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp

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

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h

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

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -125,7 +125,7 @@ public:
 
 		//backup
 		btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
-		btTransform	orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform();
+		
 		const btTransform& childTrans = compoundShape->getChildTransform(index);
 		btTransform	newChildWorldTrans = orgTrans*childTrans ;
 
@@ -235,9 +235,12 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		removeChildAlgorithms();
 		
 		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
-
+    if (m_childCollisionAlgorithms.size()==0)
+        return;
+    
 	const btDbvt* tree = compoundShape->getDynamicAabbTree();
 	//use a dynamic aabb tree to cull potential child-overlaps
 	btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
@@ -297,7 +300,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		btManifoldArray	manifoldArray;
         const btCollisionShape* childShape = 0;
         btTransform	orgTrans;
-        btTransform	orgInterpolationTrans;
+        
         btTransform	newChildWorldTrans;
         btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
         
@@ -307,8 +310,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 			{
 				childShape = compoundShape->getChildShape(i);
 			//if not longer overlapping, remove the algorithm
-                orgTrans = colObjWrap->getWorldTransform();
-                orgInterpolationTrans = colObjWrap->getWorldTransform();
+				orgTrans = colObjWrap->getWorldTransform();
+                
 				const btTransform& childTrans = compoundShape->getChildTransform(i);
                 newChildWorldTrans = orgTrans*childTrans ;
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -36,6 +36,7 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
+protected:
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -27,10 +27,8 @@ subject to the following restrictions:
 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
 
 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_sharedManifold(ci.m_manifold)
+:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
 {
-	m_ownsManifold = false;
 
 	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
 	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
@@ -114,10 +112,9 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 									btManifoldResult*	resultOut,
 									btHashedSimplePairCache* childAlgorithmsCache,
 									btPersistentManifold*	sharedManifold)
-		:m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+		:m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
 		m_childCollisionAlgorithmCache(childAlgorithmsCache),
-		m_sharedManifold(sharedManifold),
-		m_numOverlapPairs(0)
+		m_sharedManifold(sharedManifold)
 	{
 
 	}
@@ -292,12 +289,21 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 
+	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	if (!tree0 || !tree1)
+	{
+		return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
+	}
 	///btCompoundShape might have changed:
 	////make sure the internal child collision algorithm caches are still valid
 	if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
 	{
 		///clear all
 		removeChildAlgorithms();
+		m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+		m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+
 	}
 
 
@@ -329,8 +335,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	}
 
 
-	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
-	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	
 
 	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h

@@ -17,6 +17,8 @@ subject to the following restrictions:
 #ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 #define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 
+#include "btCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
@@ -35,15 +37,12 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
 extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
 
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
-class btCompoundCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm
 {
 
 	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
 	btSimplePairArray m_removePairs;
 
-	class btPersistentManifold*	m_sharedManifold;
-	bool					m_ownsManifold;
-
 
 	int	m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
 	int	m_compoundShapeRevision1;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -88,20 +88,19 @@ partId, int triangleIndex)
         //just for debugging purposes
         //printf("triangle %d",m_triangleCount++);
 
-        const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
+
 
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher1 = m_dispatcher;
 
-	//const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
-
-	
 
 
 #if 0	
+	
 	///debug drawing of the overlapping triangles
 	if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
 	{
+		const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 		btVector3 color(1,1,0);
 		btTransform& tr = ob->getWorldTransform();
 		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);

Source/ThirdParty/Bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -105,12 +105,12 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 	int maxSize = sizeof(btConvexConvexAlgorithm);
 	int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
 	int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
-	int sl = sizeof(btConvexSeparatingDistanceUtil);
-	sl = sizeof(btGjkPairDetector);
+	int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm);
+
 	int	collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
 	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
 	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
-
+	collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
 		
 	if (constructionInfo.m_persistentManifoldPool)
 	{

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.h

@@ -117,6 +117,7 @@ public:
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
 
+	SIMD_FORCE_INLINE	void	deSerializeFloat(struct btCapsuleShapeData* dataBuffer);
 
 };
 
@@ -181,4 +182,13 @@ SIMD_FORCE_INLINE	const char*	btCapsuleShape::serialize(void* dataBuffer, btSeri
 	return "btCapsuleShapeData";
 }
 
+SIMD_FORCE_INLINE	void	btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer)
+{
+	m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions);
+	m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin;
+	m_localScaling.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_localScaling);
+	//it is best to already pre-allocate the matching btCapsuleShape*(X/Z) version to match m_upAxis
+	m_upAxis = dataBuffer->m_upAxis;
+}
+
 #endif //BT_CAPSULE_SHAPE_H

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h

@@ -29,12 +29,13 @@ ATTRIBUTE_ALIGNED16(class) btCollisionShape
 protected:
 	int m_shapeType;
 	void* m_userPointer;
+	int m_userIndex;
 
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0)
+	btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1)
 	{
 	}
 
@@ -130,6 +131,16 @@ public:
 	{
 		return m_userPointer;
 	}
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
 
 	virtual	int	calculateSerializeBufferSize() const;
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp

@@ -18,7 +18,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
 #include "LinearMath/btSerializer.h"
 
-btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
+btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
 : m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
 m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
 m_dynamicAabbTree(0),
@@ -34,6 +34,8 @@ m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
 		m_dynamicAabbTree = new(mem) btDbvt();
 		btAssert(mem==m_dynamicAabbTree);
 	}
+
+	m_children.reserve(initialChildCapacity);
 }
 
 
@@ -77,8 +79,8 @@ void	btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
 	if (m_dynamicAabbTree)
 	{
 		const btDbvtVolume	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
-		int index = m_children.size();
-		child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+		size_t index = m_children.size();
+		child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) );
 	}
 
 	m_children.push_back(child);
@@ -312,7 +314,8 @@ void btCompoundShape::createAabbTreeFromChildren()
             child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
 
             const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
-            child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+			size_t index2 = index;
+            child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) );
         }
     }
 }

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -53,6 +53,7 @@ SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompou
 /// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape)
 ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
 {
+protected:
 	btAlignedObjectArray<btCompoundShapeChild> m_children;
 	btVector3						m_localAabbMin;
 	btVector3						m_localAabbMax;
@@ -64,13 +65,12 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
 
 	btScalar	m_collisionMargin;
 
-protected:
 	btVector3	m_localScaling;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btCompoundShape(bool enableDynamicAabbTree = true);
+	explicit btCompoundShape(bool enableDynamicAabbTree = true, const int initialChildCapacity = 0);
 
 	virtual ~btCompoundShape();
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -274,7 +274,7 @@ void	btConvexPolyhedron::initialize()
 #endif
 }
 
-void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
+void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 	minProj = FLT_MAX;
 	maxProj = -FLT_MAX;

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h

@@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron
 	void	initialize();
 	bool testContainment() const;
 
-	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
+	void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
 };
 
 	

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp

@@ -48,7 +48,7 @@ btConvexShape::~btConvexShape()
 }
 
 
-void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
+void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const
 {
 	btVector3 localAxis = dir*trans.getBasis();
 	btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
@@ -56,12 +56,16 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc
 
 	min = vtx1.dot(dir);
 	max = vtx2.dot(dir);
-
+	witnesPtMax = vtx2;
+	witnesPtMin = vtx1;
+	
 	if(min>max)
 	{
 		btScalar tmp = min;
 		min = max;
 		max = tmp;
+		witnesPtMax = vtx1;
+		witnesPtMin = vtx2;
 	}
 }
 

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btConvexShape.h

@@ -52,7 +52,8 @@ public:
 	btScalar getMarginNonVirtual () const;
 	void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
-	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const;
+
+	virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const;
 
 	
 	//notice that the vectors should be unit length

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -59,15 +59,13 @@ PHY_ScalarType hdt, bool flipQuadEdges
 )
 {
 	// validation
-	btAssert(heightStickWidth > 1 && "bad width");
-	btAssert(heightStickLength > 1 && "bad length");
-	btAssert(heightfieldData && "null heightfield data");
+	btAssert(heightStickWidth > 1);// && "bad width");
+	btAssert(heightStickLength > 1);// && "bad length");
+	btAssert(heightfieldData);// && "null heightfield data");
 	// btAssert(heightScale) -- do we care?  Trust caller here
-	btAssert(minHeight <= maxHeight && "bad min/max height");
-	btAssert(upAxis >= 0 && upAxis < 3 &&
-	    "bad upAxis--should be in range [0,2]");
-	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
-	    "Bad height data type enum");
+	btAssert(minHeight <= maxHeight);// && "bad min/max height");
+	btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
+	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
 
 	// initialize member variables
 	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -110,7 +108,7 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	default:
 		{
 			//need to get valid m_upAxis
-			btAssert(0 && "Bad m_upAxis");
+			btAssert(0);// && "Bad m_upAxis");
 		}
 	}
 
@@ -365,14 +363,15 @@ void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 			{
         //first triangle
         getVertex(x,j,vertices[0]);
-        getVertex(x+1,j,vertices[1]);
-        getVertex(x+1,j+1,vertices[2]);
+		getVertex(x, j + 1, vertices[1]);
+		getVertex(x + 1, j + 1, vertices[2]);
         callback->processTriangle(vertices,x,j);
         //second triangle
       //  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
         getVertex(x+1,j+1,vertices[1]);
-        getVertex(x,j+1,vertices[2]);
-        callback->processTriangle(vertices,x,j);				
+		getVertex(x + 1, j, vertices[2]);
+		callback->processTriangle(vertices, x, j);
+
 			} else
 			{
         //first triangle

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp

@@ -21,7 +21,7 @@ subject to the following restrictions:
 btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
 : btConcaveShape (), m_planeNormal(planeNormal.normalized()),
 m_planeConstant(planeConstant),
-m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
 {
 	m_shapeType = STATIC_PLANE_PROXYTYPE;
 	//	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp

@@ -75,6 +75,13 @@ void	btTriangleMesh::addIndex(int index)
 	}
 }
 
+void	btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 )
+{
+	m_indexedMeshes[0].m_numTriangles++;
+	addIndex( index1 );
+	addIndex( index2 );
+	addIndex( index3 );
+}
 
 int	btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
 {

Source/ThirdParty/Bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.h

@@ -52,7 +52,10 @@ class btTriangleMesh : public btTriangleIndexVertexArray
 		///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes.
 		///In general it is better to directly use btTriangleIndexVertexArray instead.
 		void	addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false);
-		
+
+		///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices)	
+		void	addTriangleIndices(int index1, int index2, int index3 );
+	
 		int getNumTriangles() const;
 
 		virtual void	preallocateVertices(int numverts);

Source/ThirdParty/Bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h

@@ -404,12 +404,12 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
 	CLASS_POINT & vPointA,
 	CLASS_POINT & vPointB)
 {
-    CLASS_POINT _AD,_BD,_N;
+    CLASS_POINT _AD,_BD,n;
     vec4f _M;//plane
     VEC_DIFF(_AD,vA2,vA1);
     VEC_DIFF(_BD,vB2,vB1);
-    VEC_CROSS(_N,_AD,_BD);
-    GREAL _tp = VEC_DOT(_N,_N);
+    VEC_CROSS(n,_AD,_BD);
+    GREAL _tp = VEC_DOT(n,n);
     if(_tp<G_EPSILON)//ARE PARALELE
     {
     	//project B over A
@@ -424,10 +424,10 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
     	_M[2] = VEC_DOT(vA1,_AD);
     	_M[3] = VEC_DOT(vA2,_AD);
     	//mid points
-    	_N[0] = (_M[0]+_M[1])*0.5f;
-    	_N[1] = (_M[2]+_M[3])*0.5f;
+    	n[0] = (_M[0]+_M[1])*0.5f;
+    	n[1] = (_M[2]+_M[3])*0.5f;
 
-    	if(_N[0]<_N[1])
+    	if(n[0]<n[1])
     	{
     		if(_M[1]<_M[2])
     		{
@@ -467,7 +467,7 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
     }
 
 
-    VEC_CROSS(_M,_N,_BD);
+    VEC_CROSS(_M,n,_BD);
     _M[3] = VEC_DOT(_M,vB1);
 
     LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h

@@ -0,0 +1,369 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
+#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
+
+#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
+#include "btGjkEpa3.h"
+#include "btGjkCollisionDescription.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+
+
+
+
+
+
+template <typename btConvexTemplate>
+bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
+                          const btGjkCollisionDescription& colDesc,
+                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
+{
+    (void)v;
+    
+    //	const btScalar				radialmargin(btScalar(0.));
+    
+    btVector3	guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?
+    
+    btGjkEpaSolver3::sResults	results;
+
+    
+    if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))
+        
+    {
+        //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+        //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+        wWitnessOnA = results.witnesses[0];
+        wWitnessOnB = results.witnesses[1];
+        v = results.normal;
+        return true;
+    } else
+    {
+        if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
+        {
+            wWitnessOnA = results.witnesses[0];
+            wWitnessOnB = results.witnesses[1];
+            v = results.normal;
+            return false;
+        }
+    }
+    return false;
+}
+
+template <typename btConvexTemplate, typename btGjkDistanceTemplate>
+int	btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
+{
+    
+    bool m_catchDegeneracies  = true;
+    btScalar m_cachedSeparatingDistance = 0.f;
+    
+    btScalar distance=btScalar(0.);
+    btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+    
+    btVector3 pointOnA,pointOnB;
+    btTransform	localTransA = a.getWorldTransform();
+    btTransform localTransB = b.getWorldTransform();
+    
+    btScalar marginA = a.getMargin();
+    btScalar marginB = b.getMargin();
+    
+    int m_curIter = 0;
+    int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
+    btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
+    
+    bool isValid = false;
+    bool checkSimplex = false;
+    bool checkPenetration = true;
+    int m_degenerateSimplex = 0;
+    
+    int m_lastUsedMethod = -1;
+    
+    {
+        btScalar squaredDistance = BT_LARGE_FLOAT;
+        btScalar delta = btScalar(0.);
+        
+        btScalar margin = marginA + marginB;
+        
+        
+        
+        simplexSolver.reset();
+        
+        for ( ; ; )
+            //while (true)
+        {
+            
+            btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
+            btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
+            
+            btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
+            btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
+            
+            btVector3  pWorld = localTransA(pInA);
+            btVector3  qWorld = localTransB(qInB);
+            
+            
+            
+            btVector3 w	= pWorld - qWorld;
+            delta = m_cachedSeparatingAxis.dot(w);
+            
+            // potential exit, they don't overlap
+            if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
+            {
+                m_degenerateSimplex = 10;
+                checkSimplex=true;
+                //checkPenetration = false;
+                break;
+            }
+            
+            //exit 0: the new point is already in the simplex, or we didn't come any closer
+            if (simplexSolver.inSimplex(w))
+            {
+                m_degenerateSimplex = 1;
+                checkSimplex = true;
+                break;
+            }
+            // are we getting any closer ?
+            btScalar f0 = squaredDistance - delta;
+            btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
+            
+            if (f0 <= f1)
+            {
+                if (f0 <= btScalar(0.))
+                {
+                    m_degenerateSimplex = 2;
+                } else
+                {
+                    m_degenerateSimplex = 11;
+                }
+                checkSimplex = true;
+                break;
+            }
+            
+            //add current vertex to simplex
+            simplexSolver.addVertex(w, pWorld, qWorld);
+            btVector3 newCachedSeparatingAxis;
+            
+            //calculate the closest point to the origin (update vector v)
+            if (!simplexSolver.closest(newCachedSeparatingAxis))
+            {
+                m_degenerateSimplex = 3;
+                checkSimplex = true;
+                break;
+            }
+            
+            if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
+            {
+                m_cachedSeparatingAxis = newCachedSeparatingAxis;
+                m_degenerateSimplex = 6;
+                checkSimplex = true;
+                break;
+            }
+            
+            btScalar previousSquaredDistance = squaredDistance;
+            squaredDistance = newCachedSeparatingAxis.length2();
+#if 0
+            ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
+            if (squaredDistance>previousSquaredDistance)
+            {
+                m_degenerateSimplex = 7;
+                squaredDistance = previousSquaredDistance;
+                checkSimplex = false;
+                break;
+            }
+#endif //
+            
+            
+            //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+            
+            //are we getting any closer ?
+            if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
+            {
+                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                checkSimplex = true;
+                m_degenerateSimplex = 12;
+                
+                break;
+            }
+            
+            m_cachedSeparatingAxis = newCachedSeparatingAxis;
+            
+            //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
+            if (m_curIter++ > gGjkMaxIter)
+            {
+#if defined(DEBUG) || defined (_DEBUG)
+                
+                printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
+                printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
+                       m_cachedSeparatingAxis.getX(),
+                       m_cachedSeparatingAxis.getY(),
+                       m_cachedSeparatingAxis.getZ(),
+                       squaredDistance);
+#endif
+                
+                break;
+                
+            }
+            
+            
+            bool check = (!simplexSolver.fullSimplex());
+            //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+            
+            if (!check)
+            {
+                //do we need this backup_closest here ?
+                //				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+                m_degenerateSimplex = 13;
+                break;
+            }
+        }
+        
+        if (checkSimplex)
+        {
+            simplexSolver.compute_points(pointOnA, pointOnB);
+            normalInB = m_cachedSeparatingAxis;
+            
+            btScalar lenSqr =m_cachedSeparatingAxis.length2();
+            
+            //valid normal
+            if (lenSqr < 0.0001)
+            {
+                m_degenerateSimplex = 5;
+            }
+            if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+            {
+                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+                normalInB *= rlen; //normalize
+                
+                btScalar s = btSqrt(squaredDistance);
+                
+                btAssert(s > btScalar(0.0));
+                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+                pointOnB += m_cachedSeparatingAxis * (marginB / s);
+                distance = ((btScalar(1.)/rlen) - margin);
+                isValid = true;
+                
+                m_lastUsedMethod = 1;
+            } else
+            {
+                m_lastUsedMethod = 2;
+            }
+        }
+        
+        bool catchDegeneratePenetrationCase =
+        (m_catchDegeneracies &&  m_degenerateSimplex && ((distance+margin) < 0.01));
+        
+        //if (checkPenetration && !isValid)
+        if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+        {
+            //penetration case
+            
+            //if there is no way to handle penetrations, bail out
+            
+            // Penetration depth case.
+            btVector3 tmpPointOnA,tmpPointOnB;
+            
+            m_cachedSeparatingAxis.setZero();
+            
+            bool isValid2 = btGjkEpaCalcPenDepth(a,b,
+                                                 colDesc,
+                                                 m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
+            
+            if (isValid2)
+            {
+                btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+                btScalar lenSqr = tmpNormalInB.length2();
+                if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+                {
+                    tmpNormalInB = m_cachedSeparatingAxis;
+                    lenSqr = m_cachedSeparatingAxis.length2();
+                }
+                
+                if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+                {
+                    tmpNormalInB /= btSqrt(lenSqr);
+                    btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
+                    //only replace valid penetrations when the result is deeper (check)
+                    if (!isValid || (distance2 < distance))
+                    {
+                        distance = distance2;
+                        pointOnA = tmpPointOnA;
+                        pointOnB = tmpPointOnB;
+                        normalInB = tmpNormalInB;
+                        
+                        isValid = true;
+                        m_lastUsedMethod = 3;
+                    } else
+                    {
+                        m_lastUsedMethod = 8;
+                    }
+                } else
+                {
+                    m_lastUsedMethod = 9;
+                }
+            } else
+                
+            {
+                ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
+                ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
+                ///reports a valid positive distance. Use the results of the second GJK instead of failing.
+                ///thanks to Jacob.Langford for the reproduction case
+                ///http://code.google.com/p/bullet/issues/detail?id=250
+                
+                
+                if (m_cachedSeparatingAxis.length2() > btScalar(0.))
+                {
+                    btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
+                    //only replace valid distances when the distance is less
+                    if (!isValid || (distance2 < distance))
+                    {
+                        distance = distance2;
+                        pointOnA = tmpPointOnA;
+                        pointOnB = tmpPointOnB;
+                        pointOnA -= m_cachedSeparatingAxis * marginA ;
+                        pointOnB += m_cachedSeparatingAxis * marginB ;
+                        normalInB = m_cachedSeparatingAxis;
+                        normalInB.normalize();
+                        
+                        isValid = true;
+                        m_lastUsedMethod = 6;
+                    } else
+                    {
+                        m_lastUsedMethod = 5;
+                    }
+                }
+            }
+        }
+    }
+    
+    
+    
+    if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
+    {
+        
+        m_cachedSeparatingAxis = normalInB;
+        m_cachedSeparatingDistance = distance;
+        distInfo->m_distance = distance;
+        distInfo->m_normalBtoA = normalInB;
+        distInfo->m_pointOnB = pointOnB;
+        distInfo->m_pointOnA = pointOnB+normalInB*distance;
+        return 0;
+    }
+    return -m_lastUsedMethod;
+}
+
+
+
+
+#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h

@@ -0,0 +1,41 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef GJK_COLLISION_DESCRIPTION_H
+#define GJK_COLLISION_DESCRIPTION_H
+
+#include "LinearMath/btVector3.h"
+
+struct btGjkCollisionDescription
+{
+    btVector3	m_firstDir;
+    int			m_maxGjkIterations;
+    btScalar	m_maximumDistanceSquared;
+    btScalar	m_gjkRelError2;
+    btGjkCollisionDescription()
+    :m_firstDir(0,1,0),
+    m_maxGjkIterations(1000),
+    m_maximumDistanceSquared(1e30f),
+    m_gjkRelError2(1.0e-6)
+    {
+    }
+    virtual ~btGjkCollisionDescription()
+    {
+    }
+};
+
+#endif //GJK_COLLISION_DESCRIPTION_H
+

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h

@@ -0,0 +1,1035 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2014 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the
+use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not
+claim that you wrote the original software. If you use this software in a
+product, an acknowledgment in the product documentation would be appreciated
+but is not required.
+2. Altered source versions must be plainly marked as such, and must not be
+misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+Initial GJK-EPA collision solver by Nathanael Presson, 2008
+Improvements and refactoring by Erwin Coumans, 2008-2014
+*/
+#ifndef BT_GJK_EPA3_H
+#define BT_GJK_EPA3_H
+
+#include "LinearMath/btTransform.h"
+#include "btGjkCollisionDescription.h"
+
+
+
+struct	btGjkEpaSolver3
+{
+struct	sResults
+	{
+	enum eStatus
+		{
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+		}		status;
+	btVector3	witnesses[2];
+	btVector3	normal;
+	btScalar	distance;
+	};
+
+
+};
+
+
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#endif //__SPU__
+#endif
+
+
+    
+    // Config
+    
+    /* GJK	*/
+#define GJK_MAX_ITERATIONS	128
+#define GJK_ACCURARY		((btScalar)0.0001)
+#define GJK_MIN_DISTANCE	((btScalar)0.0001)
+#define GJK_DUPLICATED_EPS	((btScalar)0.0001)
+#define GJK_SIMPLEX2_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX3_EPS	((btScalar)0.0)
+#define GJK_SIMPLEX4_EPS	((btScalar)0.0)
+    
+    /* EPA	*/
+#define EPA_MAX_VERTICES	64
+#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
+#define EPA_MAX_ITERATIONS	255
+#define EPA_ACCURACY		((btScalar)0.0001)
+#define EPA_FALLBACK		(10*EPA_ACCURACY)
+#define EPA_PLANE_EPS		((btScalar)0.00001)
+#define EPA_INSIDE_EPS		((btScalar)0.01)
+    
+    
+    // Shorthands
+    typedef unsigned int	U;
+    typedef unsigned char	U1;
+    
+    // MinkowskiDiff
+    template <typename btConvexTemplate>
+    struct	MinkowskiDiff
+    {
+        const btConvexTemplate* m_convexAPtr;
+        const btConvexTemplate* m_convexBPtr;
+        
+        btMatrix3x3				m_toshape1;
+        btTransform				m_toshape0;
+        
+        bool					m_enableMargin;
+        
+        
+        MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
+        :m_convexAPtr(&a),
+        m_convexBPtr(&b)
+        {
+        }
+        
+        void					EnableMargin(bool enable)
+        {
+            m_enableMargin = enable;
+        }
+        inline btVector3		Support0(const btVector3& d) const
+        {
+            return m_convexAPtr->getLocalSupportWithMargin(d);
+        }
+        inline btVector3		Support1(const btVector3& d) const
+        {
+            return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
+        }
+        
+        
+        inline btVector3		Support(const btVector3& d) const
+        {
+            return(Support0(d)-Support1(-d));
+        }
+        btVector3				Support(const btVector3& d,U index) const
+        {
+            if(index)
+                return(Support1(d));
+            else
+                return(Support0(d));
+        }
+    };
+    
+enum	eGjkStatus
+{
+    eGjkValid,
+    eGjkInside,
+    eGjkFailed
+};
+
+    // GJK
+    template <typename btConvexTemplate>
+    struct	GJK
+    {
+        /* Types		*/
+        struct	sSV
+        {
+            btVector3	d,w;
+        };
+        struct	sSimplex
+        {
+            sSV*		c[4];
+            btScalar	p[4];
+            U			rank;
+        };
+        
+        /* Fields		*/
+        
+        MinkowskiDiff<btConvexTemplate>			m_shape;
+        btVector3		m_ray;
+        btScalar		m_distance;
+        sSimplex		m_simplices[2];
+        sSV				m_store[4];
+        sSV*			m_free[4];
+        U				m_nfree;
+        U				m_current;
+        sSimplex*		m_simplex;
+        eGjkStatus      m_status;
+        /* Methods		*/
+        
+        GJK(const btConvexTemplate& a, const btConvexTemplate& b)
+        :m_shape(a,b)
+        {
+            Initialize();
+        }
+        void				Initialize()
+        {
+            m_ray		=	btVector3(0,0,0);
+            m_nfree		=	0;
+            m_status	=	eGjkFailed;
+            m_current	=	0;
+            m_distance	=	0;
+        }
+        eGjkStatus			Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess)
+        {
+            U			iterations=0;
+            btScalar	sqdist=0;
+            btScalar	alpha=0;
+            btVector3	lastw[4];
+            U			clastw=0;
+            /* Initialize solver		*/
+            m_free[0]			=	&m_store[0];
+            m_free[1]			=	&m_store[1];
+            m_free[2]			=	&m_store[2];
+            m_free[3]			=	&m_store[3];
+            m_nfree				=	4;
+            m_current			=	0;
+            m_status			=	eGjkValid;
+            m_shape				=	shapearg;
+            m_distance			=	0;
+            /* Initialize simplex		*/
+            m_simplices[0].rank	=	0;
+            m_ray				=	guess;
+            const btScalar	sqrl=	m_ray.length2();
+            appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
+            m_simplices[0].p[0]	=	1;
+            m_ray				=	m_simplices[0].c[0]->w;
+            sqdist				=	sqrl;
+            lastw[0]			=
+            lastw[1]			=
+            lastw[2]			=
+            lastw[3]			=	m_ray;
+            /* Loop						*/
+            do	{
+                const U		next=1-m_current;
+                sSimplex&	cs=m_simplices[m_current];
+                sSimplex&	ns=m_simplices[next];
+                /* Check zero							*/
+                const btScalar	rl=m_ray.length();
+                if(rl<GJK_MIN_DISTANCE)
+                {/* Touching or inside				*/
+                    m_status=eGjkInside;
+                    break;
+                }
+                /* Append new vertice in -'v' direction	*/
+                appendvertice(cs,-m_ray);
+                const btVector3&	w=cs.c[cs.rank-1]->w;
+                bool				found=false;
+                for(U i=0;i<4;++i)
+                {
+                    if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
+                    { found=true;break; }
+                }
+                if(found)
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                else
+                {/* Update lastw					*/
+                    lastw[clastw=(clastw+1)&3]=w;
+                }
+                /* Check for termination				*/
+                const btScalar	omega=btDot(m_ray,w)/rl;
+                alpha=btMax(omega,alpha);
+                if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                /* Reduce simplex						*/
+                btScalar	weights[4];
+                U			mask=0;
+                switch(cs.rank)
+                {
+                    case	2:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     weights,mask);break;
+                    case	3:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     cs.c[2]->w,
+                                                     weights,mask);break;
+                    case	4:	sqdist=projectorigin(	cs.c[0]->w,
+                                                     cs.c[1]->w,
+                                                     cs.c[2]->w,
+                                                     cs.c[3]->w,
+                                                     weights,mask);break;
+                }
+                if(sqdist>=0)
+                {/* Valid	*/
+                    ns.rank		=	0;
+                    m_ray		=	btVector3(0,0,0);
+                    m_current	=	next;
+                    for(U i=0,ni=cs.rank;i<ni;++i)
+                    {
+                        if(mask&(1<<i))
+                        {
+                            ns.c[ns.rank]		=	cs.c[i];
+                            ns.p[ns.rank++]		=	weights[i];
+                            m_ray				+=	cs.c[i]->w*weights[i];
+                        }
+                        else
+                        {
+                            m_free[m_nfree++]	=	cs.c[i];
+                        }
+                    }
+                    if(mask==15) m_status=eGjkInside;
+                }
+                else
+                {/* Return old simplex				*/
+                    removevertice(m_simplices[m_current]);
+                    break;
+                }
+                m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed;
+            } while(m_status==eGjkValid);
+            m_simplex=&m_simplices[m_current];
+            switch(m_status)
+            {
+                case	eGjkValid:		m_distance=m_ray.length();break;
+                case	eGjkInside:	m_distance=0;break;
+                default:
+                {
+                }
+            }
+            return(m_status);
+        }
+        bool					EncloseOrigin()
+        {
+            switch(m_simplex->rank)
+            {
+                case	1:
+                {
+                    for(U i=0;i<3;++i)
+                    {
+                        btVector3		axis=btVector3(0,0,0);
+                        axis[i]=1;
+                        appendvertice(*m_simplex, axis);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                        appendvertice(*m_simplex,-axis);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                    }
+                }
+                    break;
+                case	2:
+                {
+                    const btVector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
+                    for(U i=0;i<3;++i)
+                    {
+                        btVector3		axis=btVector3(0,0,0);
+                        axis[i]=1;
+                        const btVector3	p=btCross(d,axis);
+                        if(p.length2()>0)
+                        {
+                            appendvertice(*m_simplex, p);
+                            if(EncloseOrigin())	return(true);
+                            removevertice(*m_simplex);
+                            appendvertice(*m_simplex,-p);
+                            if(EncloseOrigin())	return(true);
+                            removevertice(*m_simplex);
+                        }
+                    }
+                }
+                    break;
+                case	3:
+                {
+                    const btVector3	n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
+                                              m_simplex->c[2]->w-m_simplex->c[0]->w);
+                    if(n.length2()>0)
+                    {
+                        appendvertice(*m_simplex,n);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                        appendvertice(*m_simplex,-n);
+                        if(EncloseOrigin())	return(true);
+                        removevertice(*m_simplex);
+                    }
+                }
+                    break;
+                case	4:
+                {
+                    if(btFabs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
+                                  m_simplex->c[1]->w-m_simplex->c[3]->w,
+                                  m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
+                        return(true);
+                }
+                    break;
+            }
+            return(false);
+        }
+        /* Internals	*/
+        void				getsupport(const btVector3& d,sSV& sv) const
+        {
+            sv.d	=	d/d.length();
+            sv.w	=	m_shape.Support(sv.d);
+        }
+        void				removevertice(sSimplex& simplex)
+        {
+            m_free[m_nfree++]=simplex.c[--simplex.rank];
+        }
+        void				appendvertice(sSimplex& simplex,const btVector3& v)
+        {
+            simplex.p[simplex.rank]=0;
+            simplex.c[simplex.rank]=m_free[--m_nfree];
+            getsupport(v,*simplex.c[simplex.rank++]);
+        }
+        static btScalar		det(const btVector3& a,const btVector3& b,const btVector3& c)
+        {
+            return(	a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
+                   a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
+                   a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          btScalar* w,U& m)
+        {
+            const btVector3	d=b-a;
+            const btScalar	l=d.length2();
+            if(l>GJK_SIMPLEX2_EPS)
+            {
+                const btScalar	t(l>0?-btDot(a,d)/l:0);
+                if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length2()); }
+                else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length2()); }
+                else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
+            }
+            return(-1);
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          const btVector3& c,
+                                          btScalar* w,U& m)
+        {
+            static const U		imd3[]={1,2,0};
+            const btVector3*	vt[]={&a,&b,&c};
+            const btVector3		dl[]={a-b,b-c,c-a};
+            const btVector3		n=btCross(dl[0],dl[1]);
+            const btScalar		l=n.length2();
+            if(l>GJK_SIMPLEX3_EPS)
+            {
+                btScalar	mindist=-1;
+                btScalar	subw[2]={0.f,0.f};
+                U			subm(0);
+                for(U i=0;i<3;++i)
+                {
+                    if(btDot(*vt[i],btCross(dl[i],n))>0)
+                    {
+                        const U			j=imd3[i];
+                        const btScalar	subd(projectorigin(*vt[i],*vt[j],subw,subm));
+                        if((mindist<0)||(subd<mindist))
+                        {
+                            mindist		=	subd;
+                            m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
+                            w[i]		=	subw[0];
+                            w[j]		=	subw[1];
+                            w[imd3[j]]	=	0;
+                        }
+                    }
+                }
+                if(mindist<0)
+                {
+                    const btScalar	d=btDot(a,n);
+                    const btScalar	s=btSqrt(l);
+                    const btVector3	p=n*(d/l);
+                    mindist	=	p.length2();
+                    m		=	7;
+                    w[0]	=	(btCross(dl[1],b-p)).length()/s;
+                    w[1]	=	(btCross(dl[2],c-p)).length()/s;
+                    w[2]	=	1-(w[0]+w[1]);
+                }
+                return(mindist);
+            }
+            return(-1);
+        }
+        static btScalar		projectorigin(	const btVector3& a,
+                                          const btVector3& b,
+                                          const btVector3& c,
+                                          const btVector3& d,
+                                          btScalar* w,U& m)
+        {
+            static const U		imd3[]={1,2,0};
+            const btVector3*	vt[]={&a,&b,&c,&d};
+            const btVector3		dl[]={a-d,b-d,c-d};
+            const btScalar		vl=det(dl[0],dl[1],dl[2]);
+            const bool			ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
+            if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
+            {
+                btScalar	mindist=-1;
+                btScalar	subw[3]={0.f,0.f,0.f};
+                U			subm(0);
+                for(U i=0;i<3;++i)
+                {
+                    const U			j=imd3[i];
+                    const btScalar	s=vl*btDot(d,btCross(dl[i],dl[j]));
+                    if(s>0)
+                    {
+                        const btScalar	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
+                        if((mindist<0)||(subd<mindist))
+                        {
+                            mindist		=	subd;
+                            m			=	static_cast<U>((subm&1?1<<i:0)+
+                                                           (subm&2?1<<j:0)+
+                                                           (subm&4?8:0));
+                            w[i]		=	subw[0];
+                            w[j]		=	subw[1];
+                            w[imd3[j]]	=	0;
+                            w[3]		=	subw[2];
+                        }
+                    }
+                }
+                if(mindist<0)
+                {
+                    mindist	=	0;
+                    m		=	15;
+                    w[0]	=	det(c,b,d)/vl;
+                    w[1]	=	det(a,c,d)/vl;
+                    w[2]	=	det(b,a,d)/vl;
+                    w[3]	=	1-(w[0]+w[1]+w[2]);
+                }
+                return(mindist);
+            }
+            return(-1);
+        }
+    };
+
+
+enum	eEpaStatus
+{
+    eEpaValid,
+    eEpaTouching,
+    eEpaDegenerated,
+    eEpaNonConvex,
+    eEpaInvalidHull,
+    eEpaOutOfFaces,
+    eEpaOutOfVertices,
+    eEpaAccuraryReached,
+    eEpaFallBack,
+    eEpaFailed
+};
+
+
+    // EPA
+template <typename btConvexTemplate>
+    struct	EPA
+    {
+        /* Types		*/
+       
+        struct	sFace
+        {
+            btVector3	n;
+            btScalar	d;
+            typename GJK<btConvexTemplate>::sSV*		c[3];
+            sFace*		f[3];
+            sFace*		l[2];
+            U1			e[3];
+            U1			pass;
+        };
+        struct	sList
+        {
+            sFace*		root;
+            U			count;
+            sList() : root(0),count(0)	{}
+        };
+        struct	sHorizon
+        {
+            sFace*		cf;
+            sFace*		ff;
+            U			nf;
+            sHorizon() : cf(0),ff(0),nf(0)	{}
+        };
+       
+        /* Fields		*/
+        eEpaStatus		m_status;
+        typename GJK<btConvexTemplate>::sSimplex	m_result;
+        btVector3		m_normal;
+        btScalar		m_depth;
+        typename GJK<btConvexTemplate>::sSV				m_sv_store[EPA_MAX_VERTICES];
+        sFace			m_fc_store[EPA_MAX_FACES];
+        U				m_nextsv;
+        sList			m_hull;
+        sList			m_stock;
+        /* Methods		*/
+        EPA()
+        {
+            Initialize();
+        }
+        
+        
+        static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
+        {
+            fa->e[ea]=(U1)eb;fa->f[ea]=fb;
+            fb->e[eb]=(U1)ea;fb->f[eb]=fa;
+        }
+        static inline void		append(sList& list,sFace* face)
+        {
+            face->l[0]	=	0;
+            face->l[1]	=	list.root;
+            if(list.root) list.root->l[0]=face;
+            list.root	=	face;
+            ++list.count;
+        }
+        static inline void		remove(sList& list,sFace* face)
+        {
+            if(face->l[1]) face->l[1]->l[0]=face->l[0];
+            if(face->l[0]) face->l[0]->l[1]=face->l[1];
+            if(face==list.root) list.root=face->l[1];
+            --list.count;
+        }
+        
+        
+        void				Initialize()
+        {
+            m_status	=	eEpaFailed;
+            m_normal	=	btVector3(0,0,0);
+            m_depth		=	0;
+            m_nextsv	=	0;
+            for(U i=0;i<EPA_MAX_FACES;++i)
+            {
+                append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
+            }
+        }
+        eEpaStatus			Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess)
+        {
+            typename GJK<btConvexTemplate>::sSimplex&	simplex=*gjk.m_simplex;
+            if((simplex.rank>1)&&gjk.EncloseOrigin())
+            {
+                
+                /* Clean up				*/
+                while(m_hull.root)
+                {
+                    sFace*	f = m_hull.root;
+                    remove(m_hull,f);
+                    append(m_stock,f);
+                }
+                m_status	=	eEpaValid;
+                m_nextsv	=	0;
+                /* Orient simplex		*/
+                if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
+                           simplex.c[1]->w-simplex.c[3]->w,
+                           simplex.c[2]->w-simplex.c[3]->w)<0)
+                {
+                    btSwap(simplex.c[0],simplex.c[1]);
+                    btSwap(simplex.p[0],simplex.p[1]);
+                }
+                /* Build initial hull	*/
+                sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
+                    newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
+                    newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
+                    newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
+                if(m_hull.count==4)
+                {
+                    sFace*		best=findbest();
+                    sFace		outer=*best;
+                    U			pass=0;
+                    U			iterations=0;
+                    bind(tetra[0],0,tetra[1],0);
+                    bind(tetra[0],1,tetra[2],0);
+                    bind(tetra[0],2,tetra[3],0);
+                    bind(tetra[1],1,tetra[3],2);
+                    bind(tetra[1],2,tetra[2],1);
+                    bind(tetra[2],2,tetra[3],1);
+                    m_status=eEpaValid;
+                    for(;iterations<EPA_MAX_ITERATIONS;++iterations)
+                    {
+                        if(m_nextsv<EPA_MAX_VERTICES)
+                        {
+                            sHorizon		horizon;
+                            typename GJK<btConvexTemplate>::sSV*			w=&m_sv_store[m_nextsv++];
+                            bool			valid=true;
+                            best->pass	=	(U1)(++pass);
+                            gjk.getsupport(best->n,*w);
+                            const btScalar	wdist=btDot(best->n,w->w)-best->d;
+                            if(wdist>EPA_ACCURACY)
+                            {
+                                for(U j=0;(j<3)&&valid;++j)
+                                {
+                                    valid&=expand(	pass,w,
+                                                  best->f[j],best->e[j],
+                                                  horizon);
+                                }
+                                if(valid&&(horizon.nf>=3))
+                                {
+                                    bind(horizon.cf,1,horizon.ff,2);
+                                    remove(m_hull,best);
+                                    append(m_stock,best);
+                                    best=findbest();
+                                    outer=*best;
+                                } else { m_status=eEpaInvalidHull;break; }
+                            } else { m_status=eEpaAccuraryReached;break; }
+                        } else { m_status=eEpaOutOfVertices;break; }
+                    }
+                    const btVector3	projection=outer.n*outer.d;
+                    m_normal	=	outer.n;
+                    m_depth		=	outer.d;
+                    m_result.rank	=	3;
+                    m_result.c[0]	=	outer.c[0];
+                    m_result.c[1]	=	outer.c[1];
+                    m_result.c[2]	=	outer.c[2];
+                    m_result.p[0]	=	btCross(	outer.c[1]->w-projection,
+                                                outer.c[2]->w-projection).length();
+                    m_result.p[1]	=	btCross(	outer.c[2]->w-projection,
+                                                outer.c[0]->w-projection).length();
+                    m_result.p[2]	=	btCross(	outer.c[0]->w-projection,
+                                                outer.c[1]->w-projection).length();
+                    const btScalar	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
+                    m_result.p[0]	/=	sum;
+                    m_result.p[1]	/=	sum;
+                    m_result.p[2]	/=	sum;
+                    return(m_status);
+                }
+            }
+            /* Fallback		*/
+            m_status	=	eEpaFallBack;
+            m_normal	=	-guess;
+            const btScalar	nl=m_normal.length();
+            if(nl>0)
+                m_normal	=	m_normal/nl;
+            else
+                m_normal	=	btVector3(1,0,0);
+            m_depth	=	0;
+            m_result.rank=1;
+            m_result.c[0]=simplex.c[0];
+            m_result.p[0]=1;
+            return(m_status);
+        }
+        bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist)
+        {
+            const btVector3 ba = b->w - a->w;
+            const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
+            const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
+            
+            if(a_dot_nab < 0)
+            {
+                // Outside of edge a->b
+                
+                const btScalar ba_l2 = ba.length2();
+                const btScalar a_dot_ba = btDot(a->w, ba);
+                const btScalar b_dot_ba = btDot(b->w, ba);
+                
+                if(a_dot_ba > 0)
+                {
+                    // Pick distance vertex a
+                    dist = a->w.length();
+                }
+                else if(b_dot_ba < 0)
+                {
+                    // Pick distance vertex b
+                    dist = b->w.length();
+                }
+                else
+                {
+                    // Pick distance to edge a->b
+                    const btScalar a_dot_b = btDot(a->w, b->w);
+                    dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
+                }
+                
+                return true;
+            }
+            
+            return false;
+        }
+        sFace*				newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced)
+        {
+            if(m_stock.root)
+            {
+                sFace*	face=m_stock.root;
+                remove(m_stock,face);
+                append(m_hull,face);
+                face->pass	=	0;
+                face->c[0]	=	a;
+                face->c[1]	=	b;
+                face->c[2]	=	c;
+                face->n		=	btCross(b->w-a->w,c->w-a->w);
+                const btScalar	l=face->n.length();
+                const bool		v=l>EPA_ACCURACY;
+                
+                if(v)
+                {
+                    if(!(getedgedist(face, a, b, face->d) ||
+                         getedgedist(face, b, c, face->d) ||
+                         getedgedist(face, c, a, face->d)))
+                    {
+                        // Origin projects to the interior of the triangle
+                        // Use distance to triangle plane
+                        face->d = btDot(a->w, face->n) / l;
+                    }
+                    
+                    face->n /= l;
+                    if(forced || (face->d >= -EPA_PLANE_EPS))
+                    {
+                        return face;
+                    }
+                    else
+                        m_status=eEpaNonConvex;
+                }
+                else
+                    m_status=eEpaDegenerated;
+                
+                remove(m_hull, face);
+                append(m_stock, face);
+                return 0;
+                
+            }
+            m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
+            return 0;
+        }
+        sFace*				findbest()
+        {
+            sFace*		minf=m_hull.root;
+            btScalar	mind=minf->d*minf->d;
+            for(sFace* f=minf->l[1];f;f=f->l[1])
+            {
+                const btScalar	sqd=f->d*f->d;
+                if(sqd<mind)
+                {
+                    minf=f;
+                    mind=sqd;
+                }
+            }
+            return(minf);
+        }
+        bool				expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon)
+        {
+            static const U	i1m3[]={1,2,0};
+            static const U	i2m3[]={2,0,1};
+            if(f->pass!=pass)
+            {
+                const U	e1=i1m3[e];
+                if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
+                {
+                    sFace*	nf=newface(f->c[e1],f->c[e],w,false);
+                    if(nf)
+                    {
+                        bind(nf,0,f,e);
+                        if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
+                        horizon.cf=nf;
+                        ++horizon.nf;
+                        return(true);
+                    }
+                }
+                else
+                {
+                    const U	e2=i2m3[e];
+                    f->pass		=	(U1)pass;
+                    if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
+                       expand(pass,w,f->f[e2],f->e[e2],horizon))
+                    {
+                        remove(m_hull,f);
+                        append(m_stock,f);
+                        return(true);
+                    }
+                }
+            }
+            return(false);
+        }
+        
+    };
+    
+    template <typename btConvexTemplate>
+    static void	Initialize(	const btConvexTemplate& a, const btConvexTemplate& b,
+                           btGjkEpaSolver3::sResults& results,
+                           MinkowskiDiff<btConvexTemplate>& shape)
+    {
+        /* Results		*/ 
+        results.witnesses[0]	=
+        results.witnesses[1]	=	btVector3(0,0,0);
+        results.status			=	btGjkEpaSolver3::sResults::Separated;
+        /* Shape		*/ 
+       
+        shape.m_toshape1		=	b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
+        shape.m_toshape0		=	a.getWorldTransform().inverseTimes(b.getWorldTransform());
+        
+    }
+    
+
+//
+// Api
+//
+
+
+
+//
+template <typename btConvexTemplate>
+bool		btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
+                                      const btVector3& guess,
+                                      btGjkEpaSolver3::sResults& results)
+{
+    MinkowskiDiff<btConvexTemplate>			shape(a,b);
+    Initialize(a,b,results,shape);
+    GJK<btConvexTemplate>				gjk(a,b);
+    eGjkStatus	gjk_status=gjk.Evaluate(shape,guess);
+    if(gjk_status==eGjkValid)
+    {
+        btVector3	w0=btVector3(0,0,0);
+        btVector3	w1=btVector3(0,0,0);
+        for(U i=0;i<gjk.m_simplex->rank;++i)
+        {
+            const btScalar	p=gjk.m_simplex->p[i];
+            w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
+            w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+        }
+        results.witnesses[0]	=	a.getWorldTransform()*w0;
+        results.witnesses[1]	=	a.getWorldTransform()*w1;
+        results.normal			=	w0-w1;
+        results.distance		=	results.normal.length();
+        results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
+        return(true);
+    }
+    else
+    {
+        results.status	=	gjk_status==eGjkInside?
+        btGjkEpaSolver3::sResults::Penetrating	:
+        btGjkEpaSolver3::sResults::GJK_Failed	;
+        return(false);
+    }
+}
+
+
+template <typename btConvexTemplate>
+bool	btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
+                                     const btConvexTemplate& b,
+                                     const btVector3& guess,
+                                     btGjkEpaSolver3::sResults& results)
+{
+    MinkowskiDiff<btConvexTemplate>			shape(a,b);
+    Initialize(a,b,results,shape);
+    GJK<btConvexTemplate>				gjk(a,b);
+    eGjkStatus	gjk_status=gjk.Evaluate(shape,-guess);
+    switch(gjk_status)
+    {
+        case	eGjkInside:
+        {
+            EPA<btConvexTemplate>				epa;
+            eEpaStatus	epa_status=epa.Evaluate(gjk,-guess);
+            if(epa_status!=eEpaFailed)
+            {
+                btVector3	w0=btVector3(0,0,0);
+                for(U i=0;i<epa.m_result.rank;++i)
+                {
+                    w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+                }
+                results.status			=	btGjkEpaSolver3::sResults::Penetrating;
+                results.witnesses[0]	=	a.getWorldTransform()*w0;
+                results.witnesses[1]	=	a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
+                results.normal			=	-epa.m_normal;
+                results.distance		=	-epa.m_depth;
+                return(true);
+            } else results.status=btGjkEpaSolver3::sResults::EPA_Failed;
+        }
+            break;
+        case	eGjkFailed:
+            results.status=btGjkEpaSolver3::sResults::GJK_Failed;
+            break;
+        default:
+        {
+        }
+    }
+    return(false);
+}
+
+#if 0
+int	btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
+{
+    btGjkEpaSolver3::sResults results;
+    btVector3 guess = colDesc.m_firstDir;
+    
+    bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
+                                            colDesc.m_transformA,colDesc.m_transformB,
+                                            colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
+                                            guess,
+                                            results);
+    if (res)
+    {
+        if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
+        {
+            //normal could be 'swapped'
+            
+            distInfo->m_distance = results.distance;
+            distInfo->m_normalBtoA = results.normal;
+            btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
+            btScalar lenSqr = tmpNormalInB.length2();
+            if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+            {
+                tmpNormalInB = results.normal;
+                lenSqr = results.normal.length2();
+            }
+            
+            if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+            {
+                tmpNormalInB /= btSqrt(lenSqr);
+                btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
+                //only replace valid penetrations when the result is deeper (check)
+                //if ((distance2 < results.distance))
+                {
+                    distInfo->m_distance = distance2;
+                    distInfo->m_pointOnA= results.witnesses[0];
+                    distInfo->m_pointOnB= results.witnesses[1];
+                    distInfo->m_normalBtoA= tmpNormalInB;
+                    return 0;
+                }
+            }
+        }
+        
+    }
+    
+    return -1;
+}
+#endif
+
+template <typename btConvexTemplate, typename btDistanceInfoTemplate>
+int	btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
+{
+    btGjkEpaSolver3::sResults results;
+    btVector3 guess = colDesc.m_firstDir;
+    
+    bool isSeparated = btGjkEpaSolver3_Distance(	a,b,
+                                                 guess,
+                                                 results);
+    if (isSeparated)
+    {
+        distInfo->m_distance = results.distance;
+        distInfo->m_pointOnA= results.witnesses[0];
+        distInfo->m_pointOnB= results.witnesses[1];
+        distInfo->m_normalBtoA= results.normal;
+        return 0;
+    }
+    
+    return -1;
+}
+
+/* Symbols cleanup		*/ 
+
+#undef GJK_MAX_ITERATIONS
+#undef GJK_ACCURARY
+#undef GJK_MIN_DISTANCE
+#undef GJK_DUPLICATED_EPS
+#undef GJK_SIMPLEX2_EPS
+#undef GJK_SIMPLEX3_EPS
+#undef GJK_SIMPLEX4_EPS
+
+#undef EPA_MAX_VERTICES
+#undef EPA_MAX_FACES
+#undef EPA_MAX_ITERATIONS
+#undef EPA_ACCURACY
+#undef EPA_FALLBACK
+#undef EPA_PLANE_EPS
+#undef EPA_INSIDE_EPS
+
+
+
+#endif //BT_GJK_EPA3_H
+

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h

@@ -0,0 +1,908 @@
+
+/***
+ * ---------------------------------
+ * 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.
+ *  The original MPR idea and implementation is by Gary Snethen
+ *  in XenoCollide, see http://github.com/erwincoumans/xenocollide
+ *
+ *  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.
+ */
+
+///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
+
+#ifndef BT_MPR_PENETRATION_H
+#define BT_MPR_PENETRATION_H
+
+#define BT_DEBUG_MPR1
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+//#define MPR_AVERAGE_CONTACT_POSITIONS
+
+
+struct btMprCollisionDescription
+{
+    btVector3	m_firstDir;
+    int			m_maxGjkIterations;
+    btScalar	m_maximumDistanceSquared;
+    btScalar	m_gjkRelError2;
+   
+    btMprCollisionDescription()
+    :	m_firstDir(0,1,0),
+        m_maxGjkIterations(1000),
+        m_maximumDistanceSquared(1e30f),
+        m_gjkRelError2(1.0e-6)
+    {
+    }
+    virtual ~btMprCollisionDescription()
+    {
+    }
+};
+
+struct btMprDistanceInfo
+{
+    btVector3	m_pointOnA;
+    btVector3	m_pointOnB;
+    btVector3	m_normalBtoA;
+    btScalar	m_distance;
+};
+
+#ifdef __cplusplus
+#define BT_MPR_SQRT sqrtf
+#else
+#define BT_MPR_SQRT sqrt
+#endif
+#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
+#define BT_MPR_FABS fabs
+
+#define BT_MPR_TOLERANCE 1E-6f
+#define BT_MPR_MAX_ITERATIONS 1000
+
+struct _btMprSupport_t 
+{
+    btVector3 v;  //!< Support point in minkowski sum
+    btVector3 v1; //!< Support point in obj1
+    btVector3 v2; //!< Support point in obj2
+};
+typedef struct _btMprSupport_t btMprSupport_t;
+
+struct _btMprSimplex_t 
+{
+    btMprSupport_t ps[4];
+    int last; //!< index of last added point
+};
+typedef struct _btMprSimplex_t btMprSimplex_t;
+
+inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
+{
+    return &s->ps[idx];
+}
+
+inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
+{
+    s->last = size - 1;
+}
+
+#ifdef DEBUG_MPR
+inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
+{
+    printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(),
+           portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(),
+           portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z());
+}
+#endif //DEBUG_MPR
+
+
+
+
+inline int btMprSimplexSize(const btMprSimplex_t *s)
+{
+    return s->last + 1;
+}
+
+
+inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
+{
+    // here is no check on boundaries
+    return &s->ps[idx];
+}
+
+inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
+{
+    *d = *s;
+}
+
+inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
+{
+    btMprSupportCopy(s->ps + pos, a);
+}
+
+
+inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
+{
+    btMprSupport_t supp;
+
+    btMprSupportCopy(&supp, &s->ps[pos1]);
+    btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+    btMprSupportCopy(&s->ps[pos2], &supp);
+}
+
+
+inline int btMprIsZero(float val)
+{
+    return BT_MPR_FABS(val) < FLT_EPSILON;
+}
+
+
+
+inline int btMprEq(float _a, float _b)
+{
+    float ab;
+    float a, b;
+
+    ab = BT_MPR_FABS(_a - _b);
+    if (BT_MPR_FABS(ab) < FLT_EPSILON)
+        return 1;
+
+    a = BT_MPR_FABS(_a);
+    b = BT_MPR_FABS(_b);
+    if (b > a){
+        return ab < FLT_EPSILON * b;
+    }else{
+        return ab < FLT_EPSILON * a;
+    }
+}
+
+
+inline int btMprVec3Eq(const btVector3* a, const btVector3 *b)
+{
+    return btMprEq((*a).x(), (*b).x())
+            && btMprEq((*a).y(), (*b).y())
+            && btMprEq((*a).z(), (*b).z());
+}
+
+
+
+
+
+
+
+
+
+
+
+template <typename btConvexTemplate>
+inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center)
+{
+
+	center->v1 = a.getObjectCenterInWorld();
+    center->v2 = b.getObjectCenterInWorld();
+    center->v = center->v1 - center->v2;
+}
+
+inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
+{
+	v->setValue(x,y,z);
+}
+
+inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
+{
+	*v += *w;
+}
+
+inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
+{
+    *v = *w;
+}
+
+inline void btMprVec3Scale(btVector3 *d, float k)
+{
+    *d *= k;
+}
+
+inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
+{
+    float dot;
+
+	dot = btDot(*a,*b);
+    return dot;
+}
+
+
+inline float btMprVec3Len2(const btVector3 *v)
+{
+    return btMprVec3Dot(v, v);
+}
+
+inline void btMprVec3Normalize(btVector3 *d)
+{
+    float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
+    btMprVec3Scale(d, k);
+}
+
+inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
+{
+	*d = btCross(*a,*b);
+	
+}
+
+
+inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
+{
+	*d = *v - *w;
+}
+
+inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
+{
+    btVector3 v2v1, v3v1;
+
+    btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    btMprVec3Cross(dir, &v2v1, &v3v1);
+    btMprVec3Normalize(dir);
+}
+
+
+inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
+                                       const btVector3 *dir)
+{
+    float dot;
+    dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
+    return btMprIsZero(dot) || dot > 0.f;
+}
+
+inline int portalReachTolerance(const btMprSimplex_t *portal,
+                                     const btMprSupport_t *v4,
+                                     const btVector3 *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 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
+    dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
+    dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
+    dv4 = btMprVec3Dot(&v4->v, dir);
+
+    dot1 = dv4 - dv1;
+    dot2 = dv4 - dv2;
+    dot3 = dv4 - dv3;
+
+    dot1 = BT_MPR_FMIN(dot1, dot2);
+    dot1 = BT_MPR_FMIN(dot1, dot3);
+
+    return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
+}
+
+inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
+                                         const btMprSupport_t *v4,
+                                         const btVector3 *dir)
+{
+    float dot;
+    dot = btMprVec3Dot(&v4->v, dir);
+    return btMprIsZero(dot) || dot > 0.f;
+}
+
+inline void btExpandPortal(btMprSimplex_t *portal,
+                              const btMprSupport_t *v4)
+{
+    float dot;
+    btVector3 v4v0;
+
+    btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
+    if (dot > 0.f){
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
+        if (dot > 0.f){
+            btMprSimplexSet(portal, 1, v4);
+        }else{
+            btMprSimplexSet(portal, 3, v4);
+        }
+    }else{
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
+        if (dot > 0.f){
+            btMprSimplexSet(portal, 2, v4);
+        }else{
+            btMprSimplexSet(portal, 1, v4);
+        }
+    }
+}
+template <typename btConvexTemplate>
+inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btMprCollisionDescription& colDesc,
+													const btVector3& dir, btMprSupport_t *supp)
+{
+	btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
+	btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();
+
+	btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
+	btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
+
+	supp->v1 = a.getWorldTransform()(pInA);
+	supp->v2 = b.getWorldTransform()(qInB);
+	supp->v = supp->v1 - supp->v2;
+}
+
+
+template <typename btConvexTemplate>
+static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b,
+                            const btMprCollisionDescription& colDesc,
+													btMprSimplex_t *portal)
+{
+    btVector3 dir, va, vb;
+    float dot;
+    int cont;
+	
+	
+
+    // vertex 0 is center of portal
+    btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0));
+   
+    
+    // vertex 0 is center of portal
+    btMprSimplexSetSize(portal, 1);
+    
+
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* org = &zero;
+
+    if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){
+        // 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...
+        btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+        btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
+    }
+
+
+    // vertex 1 = support in direction of origin
+    btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Scale(&dir, -1.f);
+    btMprVec3Normalize(&dir);
+
+
+    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1));
+ 
+    btMprSimplexSetSize(portal, 2);
+
+    // test if origin isn't outside of v1
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
+	
+
+    if (btMprIsZero(dot) || dot < 0.f)
+        return -1;
+
+
+    // vertex 2
+    btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    if (btMprIsZero(btMprVec3Len2(&dir))){
+        if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){
+            // origin lies on v1
+            return 1;
+        }else{
+            // origin lies on v0-v1 segment
+            return 2;
+        }
+    }
+
+    btMprVec3Normalize(&dir);
+    btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2));
+    
+    
+    
+    dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
+    if (btMprIsZero(dot) || dot < 0.f)
+        return -1;
+
+    btMprSimplexSetSize(portal, 3);
+
+    // vertex 3 direction
+    btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
+                     &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
+                     &btMprSimplexPoint(portal, 0)->v);
+    btMprVec3Cross(&dir, &va, &vb);
+    btMprVec3Normalize(&dir);
+
+    // it is better to form portal faces to be oriented "outside" origin
+    dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
+    if (dot > 0.f){
+        btMprSimplexSwap(portal, 1, 2);
+        btMprVec3Scale(&dir, -1.f);
+    }
+
+    while (btMprSimplexSize(portal) < 4){
+		 btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3));
+        
+        dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
+        if (btMprIsZero(dot) || dot < 0.f)
+            return -1;
+
+        cont = 0;
+
+        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
+        // continue
+        btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
+                          &btMprSimplexPoint(portal, 3)->v);
+        dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
+        if (dot < 0.f && !btMprIsZero(dot)){
+            btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
+            cont = 1;
+        }
+
+        if (!cont){
+            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
+            // continue
+            btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
+                              &btMprSimplexPoint(portal, 2)->v);
+            dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
+            if (dot < 0.f && !btMprIsZero(dot)){
+                btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
+                cont = 1;
+            }
+        }
+
+        if (cont){
+            btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
+                             &btMprSimplexPoint(portal, 0)->v);
+            btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
+                             &btMprSimplexPoint(portal, 0)->v);
+            btMprVec3Cross(&dir, &va, &vb);
+            btMprVec3Normalize(&dir);
+        }else{
+            btMprSimplexSetSize(portal, 4);
+        }
+    }
+
+    return 0;
+}
+
+template <typename btConvexTemplate>
+static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
+							btMprSimplex_t *portal)
+{
+    btVector3 dir;
+    btMprSupport_t v4;
+
+	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute direction outside the portal (from v0 throught v1,v2,v3
+        // face)
+        btPortalDir(portal, &dir);
+
+        // test if origin is inside the portal
+        if (portalEncapsulesOrigin(portal, &dir))
+            return 0;
+
+        // get next support point
+        
+		 btMprSupport(a,b,colDesc, 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).
+        btExpandPortal(portal, &v4);
+    }
+
+    return -1;
+}
+
+static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
+{
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+    btVector3 dir;
+    size_t i;
+    float b[4], sum, inv;
+    btVector3 vec, p1, p2;
+
+    btPortalDir(portal, &dir);
+
+    // use barycentric coordinates of tetrahedron to find origin
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
+                       &btMprSimplexPoint(portal, 2)->v);
+    b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
+                       &btMprSimplexPoint(portal, 2)->v);
+    b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
+
+    btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
+                       &btMprSimplexPoint(portal, 1)->v);
+    b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+    if (btMprIsZero(sum) || sum < 0.f){
+		b[0] = 0.f;
+
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
+                           &btMprSimplexPoint(portal, 3)->v);
+        b[1] = btMprVec3Dot(&vec, &dir);
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
+                           &btMprSimplexPoint(portal, 1)->v);
+        b[2] = btMprVec3Dot(&vec, &dir);
+        btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
+                           &btMprSimplexPoint(portal, 2)->v);
+        b[3] = btMprVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = 1.f / sum;
+
+    btMprVec3Copy(&p1, origin);
+    btMprVec3Copy(&p2, origin);
+    for (i = 0; i < 4; i++){
+        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
+        btMprVec3Scale(&vec, b[i]);
+        btMprVec3Add(&p1, &vec);
+
+        btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
+        btMprVec3Scale(&vec, b[i]);
+        btMprVec3Add(&p2, &vec);
+    }
+    btMprVec3Scale(&p1, inv);
+    btMprVec3Scale(&p2, inv);
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &p1);
+    btMprVec3Add(pos, &p2);
+    btMprVec3Scale(pos, 0.5);
+#else
+    btMprVec3Copy(pos, &p2);
+#endif//MPR_AVERAGE_CONTACT_POSITIONS
+}
+
+inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
+{
+    btVector3 ab;
+    btMprVec3Sub2(&ab, a, b);
+    return btMprVec3Len2(&ab);
+}
+
+inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
+                                                  const btVector3 *x0,
+                                                  const btVector3 *b,
+                                                  btVector3 *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;
+    btVector3 d, a;
+
+    // direction of segment
+    btMprVec3Sub2(&d, b, x0);
+
+    // precompute vector from P to x0
+    btMprVec3Sub2(&a, x0, P);
+
+    t  = -1.f * btMprVec3Dot(&a, &d);
+    t /= btMprVec3Len2(&d);
+
+    if (t < 0.f || btMprIsZero(t)){
+        dist = btMprVec3Dist2(x0, P);
+        if (witness)
+            btMprVec3Copy(witness, x0);
+    }else if (t > 1.f || btMprEq(t, 1.f)){
+        dist = btMprVec3Dist2(b, P);
+        if (witness)
+            btMprVec3Copy(witness, b);
+    }else{
+        if (witness){
+            btMprVec3Copy(witness, &d);
+            btMprVec3Scale(witness, t);
+            btMprVec3Add(witness, x0);
+            dist = btMprVec3Dist2(witness, P);
+        }else{
+            // recycling variables
+            btMprVec3Scale(&d, t);
+            btMprVec3Add(&d, &a);
+            dist = btMprVec3Len2(&d);
+        }
+    }
+
+    return dist;
+}
+
+
+
+inline float btMprVec3PointTriDist2(const btVector3 *P,
+                                const btVector3 *x0, const btVector3 *B,
+                                const btVector3 *C,
+                                btVector3 *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.
+
+    btVector3 d1, d2, a;
+    float u, v, w, p, q, r;
+    float s, t, dist, dist2;
+    btVector3 witness2;
+
+    btMprVec3Sub2(&d1, B, x0);
+    btMprVec3Sub2(&d2, C, x0);
+    btMprVec3Sub2(&a, x0, P);
+
+    u = btMprVec3Dot(&a, &a);
+    v = btMprVec3Dot(&d1, &d1);
+    w = btMprVec3Dot(&d2, &d2);
+    p = btMprVec3Dot(&a, &d1);
+    q = btMprVec3Dot(&a, &d2);
+    r = btMprVec3Dot(&d1, &d2);
+
+	btScalar div = (w * v - r * r);
+	if (btMprIsZero(div))
+	{
+		s=-1;
+	} else
+	{
+		s = (q * r - w * p) / div;
+		t = (-s * r - q) / w;
+	}
+
+    if ((btMprIsZero(s) || s > 0.f)
+            && (btMprEq(s, 1.f) || s < 1.f)
+            && (btMprIsZero(t) || t > 0.f)
+            && (btMprEq(t, 1.f) || t < 1.f)
+            && (btMprEq(t + s, 1.f) || t + s < 1.f)){
+
+        if (witness){
+            btMprVec3Scale(&d1, s);
+            btMprVec3Scale(&d2, t);
+            btMprVec3Copy(witness, x0);
+            btMprVec3Add(witness, &d1);
+            btMprVec3Add(witness, &d2);
+
+            dist = btMprVec3Dist2(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 = _btMprVec3PointSegmentDist2(P, x0, B, witness);
+
+        dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                btMprVec3Copy(witness, &witness2);
+        }
+
+        dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
+        if (dist2 < dist){
+            dist = dist2;
+            if (witness)
+                btMprVec3Copy(witness, &witness2);
+        }
+    }
+
+    return dist;
+}
+
+template <typename btConvexTemplate>
+static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b,
+                         const btMprCollisionDescription& colDesc,
+                         btMprSimplex_t *portal,
+                         float *depth, btVector3 *pdir, btVector3 *pos)
+{
+    btVector3 dir;
+    btMprSupport_t v4;
+    unsigned long iterations;
+
+	btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+
+    iterations = 1UL;
+	for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
+    //while (1)
+	{
+        // compute portal direction and obtain next support point
+        btPortalDir(portal, &dir);
+        
+		 btMprSupport(a,b,colDesc, dir, &v4);
+
+
+        // reached tolerance -> find penetration info
+        if (portalReachTolerance(portal, &v4, &dir)
+                || iterations ==BT_MPR_MAX_ITERATIONS)
+		{
+            *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir);
+            *depth = BT_MPR_SQRT(*depth);
+			
+			if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
+			{
+				
+				*pdir = dir;
+			} 
+			btMprVec3Normalize(pdir);
+			
+            // barycentric coordinates:
+            btFindPos(portal, pos);
+
+
+            return;
+        }
+
+        btExpandPortal(portal, &v4);
+
+        iterations++;
+    }
+}
+
+static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
+{
+    // Touching contact on portal's v1 - so depth is zero and direction
+    // is unimportant and pos can be guessed
+    *depth = 0.f;
+    btVector3 zero = btVector3(0,0,0);
+	btVector3* origin = &zero;
+
+
+	btMprVec3Copy(dir, origin);
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
+    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
+    btMprVec3Scale(pos, 0.5);
+#else
+     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
+#endif
+}
+
+static void btFindPenetrSegment(btMprSimplex_t *portal,
+                              float *depth, btVector3 *dir, btVector3 *pos)
+{
+    
+    // Origin lies on v0-v1 segment.
+    // Depth is distance to v1, direction also and position must be
+    // computed
+#ifdef MPR_AVERAGE_CONTACT_POSITIONS
+    btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
+    btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
+    btMprVec3Scale(pos, 0.5f);
+#else
+     btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
+#endif//MPR_AVERAGE_CONTACT_POSITIONS
+
+    btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
+    *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
+    btMprVec3Normalize(dir);
+
+    
+}
+
+
+template <typename btConvexTemplate>
+inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b,
+                            const btMprCollisionDescription& colDesc,
+					float *depthOut, btVector3* dirOut, btVector3* posOut)
+{
+	
+	 btMprSimplex_t portal;
+
+
+    // Phase 1: Portal discovery
+    int result = btDiscoverPortal(a,b,colDesc, &portal);
+	
+	  
+	//sepAxis[pairIndex] = *pdir;//or -dir?
+
+	switch (result)
+	{
+	case 0:
+		{
+			// Phase 2: Portal refinement
+		
+			result = btRefinePortal(a,b,colDesc, &portal);
+			if (result < 0)
+				return -1;
+
+			// Phase 3. Penetration info
+			btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
+			
+			
+			break;
+		}
+	case 1:
+		{
+			 // Touching contact on portal's v1.
+			btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
+			result=0;
+			break;
+		}
+	case 2:
+		{
+			
+			btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
+			result=0;
+			break;
+		}
+	default:
+		{
+			//if (res < 0)
+			//{
+				// Origin isn't inside portal - no collision.
+				result = -1;
+			//}
+		}
+	};
+	
+	return result;
+};
+
+
+template<typename btConvexTemplate, typename btMprDistanceTemplate>
+inline int	btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const
+                                    btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo)
+{
+	btVector3 dir,pos;
+	float depth;
+
+	int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
+	if (res==0)
+	{
+		distInfo->m_distance = -depth;
+		distInfo->m_pointOnB = pos;
+		distInfo->m_normalBtoA = -dir;
+		distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
+		return 0;
+	}
+
+	return -1;
+}
+
+
+
+#endif //BT_MPR_PENETRATION_H

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -178,62 +178,62 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 }
 #endif //TEST_INTERNAL_OBJECTS
 
- 
- 
- SIMD_FORCE_INLINE void btSegmentsClosestPoints(
-	btVector3& ptsVector,
-	btVector3& offsetA,
-	btVector3& offsetB,
-	btScalar& tA, btScalar& tB,
-	const btVector3& translation,
-	const btVector3& dirA, btScalar hlenA,
-	const btVector3& dirB, btScalar hlenB )
-{
-	// compute the parameters of the closest points on each line segment
-
-	btScalar dirA_dot_dirB = btDot(dirA,dirB);
-	btScalar dirA_dot_trans = btDot(dirA,translation);
-	btScalar dirB_dot_trans = btDot(dirB,translation);
-
-	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
-
-	if ( denom == 0.0f ) {
-		tA = 0.0f;
-	} else {
-		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	}
-
-	tB = tA * dirA_dot_dirB - dirB_dot_trans;
-
-	if ( tB < -hlenB ) {
-		tB = -hlenB;
-		tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	} else if ( tB > hlenB ) {
-		tB = hlenB;
-		tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
-		if ( tA < -hlenA )
-			tA = -hlenA;
-		else if ( tA > hlenA )
-			tA = hlenA;
-	}
-
-	// compute the closest points relative to segment centers.
-
-	offsetA = dirA * tA;
-	offsetB = dirB * tB;
-
-	ptsVector = translation - offsetA + offsetB;
-}
+ 
+ 
+ SIMD_FORCE_INLINE void btSegmentsClosestPoints(
+	btVector3& ptsVector,
+	btVector3& offsetA,
+	btVector3& offsetB,
+	btScalar& tA, btScalar& tB,
+	const btVector3& translation,
+	const btVector3& dirA, btScalar hlenA,
+	const btVector3& dirB, btScalar hlenB )
+{
+	// compute the parameters of the closest points on each line segment
+
+	btScalar dirA_dot_dirB = btDot(dirA,dirB);
+	btScalar dirA_dot_trans = btDot(dirA,translation);
+	btScalar dirB_dot_trans = btDot(dirB,translation);
+
+	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+	if ( denom == 0.0f ) {
+		tA = 0.0f;
+	} else {
+		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+	if ( tB < -hlenB ) {
+		tB = -hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	} else if ( tB > hlenB ) {
+		tB = hlenB;
+		tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+		if ( tA < -hlenA )
+			tA = -hlenA;
+		else if ( tA > hlenA )
+			tA = hlenA;
+	}
+
+	// compute the closest points relative to segment centers.
+
+	offsetA = dirA * tA;
+	offsetB = dirB * tB;
+
+	ptsVector = translation - offsetA + offsetB;
+}
 
 
 
@@ -313,7 +313,7 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	int edgeB=-1;
 	btVector3 worldEdgeA;
 	btVector3 worldEdgeB;
-	btVector3 witnessPointA,witnessPointB;
+	btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
 	
 
 	int curEdgeEdge = 0;
@@ -369,23 +369,23 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 //		printf("edge-edge\n");
 		//add an edge-edge contact
 
-		btVector3 ptsVector;
-		btVector3 offsetA;
-		btVector3 offsetB;
-		btScalar tA;
-		btScalar tB;
-
-		btVector3 translation = witnessPointB-witnessPointA;
-
-		btVector3 dirA = worldEdgeA;
-		btVector3 dirB = worldEdgeB;
-		
-		btScalar hlenB = 1e30f;
-		btScalar hlenA = 1e30f;
-
-		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
-			translation,
-			dirA, hlenA,
+		btVector3 ptsVector;
+		btVector3 offsetA;
+		btVector3 offsetB;
+		btScalar tA;
+		btScalar tB;
+
+		btVector3 translation = witnessPointB-witnessPointA;
+
+		btVector3 dirA = worldEdgeA;
+		btVector3 dirB = worldEdgeB;
+		
+		btScalar hlenB = 1e30f;
+		btScalar hlenA = 1e30f;
+
+		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
+			translation,
+			dirA, hlenA,
 			dirB,hlenB);
 
 		btScalar nlSqrt = ptsVector.length2();

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h

@@ -32,10 +32,12 @@ public:
    //@BP Mod - allow backface filtering and unflipped normals
    enum EFlags
    {
-      kF_None                 = 0,
+	  kF_None                 = 0,
       kF_FilterBackfaces      = 1 << 0,
       kF_KeepUnflippedNormal  = 1 << 1,   // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
-	  kF_UseSubSimplexConvexCastRaytest =  1 << 2,   // Uses an approximate but faster ray versus convex intersection algorithm
+		///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
+	  kF_UseSubSimplexConvexCastRaytest = 1 << 2,   // Uses an approximate but faster ray versus convex intersection algorithm
+	  kF_UseGjkConvexCastRaytest = 1 << 3,
       kF_Terminator        = 0xFFFFFFFF
    };
    unsigned int m_flags;

Source/ThirdParty/Bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -65,10 +65,10 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 
 	btVector3 n;
 	n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	bool hasResult = false;
+	
 	btVector3 c;
 
-	btScalar lastLambda = lambda;
+	
 
 
 	btScalar dist2 = v.length2();
@@ -109,9 +109,9 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 				//m_simplexSolver->reset();
 				//check next line
 				 w = supVertexA-supVertexB;
-				lastLambda = lambda;
+				
 				n = v;
-				hasResult = true;
+				
 			}
 		} 
 		///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
@@ -121,7 +121,7 @@ bool	btSubsimplexConvexCast::calcTimeOfImpact(
 		if (m_simplexSolver->closest(v))
 		{
 			dist2 = v.length2();
-			hasResult = true;
+			
 			//todo: check this normal for validity
 			//n=v;
 			//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);

Source/ThirdParty/Bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -29,9 +29,10 @@ subject to the following restrictions:
 static btVector3
 getNormalizedVector(const btVector3& v)
 {
-	btVector3 n = v.normalized();
-	if (n.length() < SIMD_EPSILON) {
-		n.setValue(0, 0, 0);
+	btVector3 n(0, 0, 0);
+
+	if (v.length() > SIMD_EPSILON) {
+		n = v.normalized();
 	}
 	return n;
 }
@@ -380,8 +381,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		if (callback.hasHit())
 		{	
 			// we moved only a fraction
-			btScalar hitDistance;
-			hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+			//btScalar hitDistance;
+			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
 //			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
@@ -635,7 +636,7 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 //	printf("  dt = %f", dt);
 
 	// quick check...
-	if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
 //		printf("\n");
 		return;		// no motion
 	}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo()
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	btVector3 b1Axis1,b1Axis2,b1Axis3;
-	btVector3 b2Axis1,b2Axis2;
+	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
+	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
 
 	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
 	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
@@ -778,8 +778,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
 				target.normalize();
 				m_swingAxis = -ivB.cross(target);
-				m_swingCorrection = m_swingAxis.length();
-				m_swingAxis.normalize();
+                                m_swingCorrection = m_swingAxis.length();
+
+                                if (!btFuzzyZero(m_swingCorrection))
+                                    m_swingAxis.normalize();
 			}
 		}
 
@@ -983,8 +985,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA
 
 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
 {
-	btTransform trACur = m_rbA.getCenterOfMassTransform();
-	btTransform trBCur = m_rbB.getCenterOfMassTransform();
+	//btTransform trACur = m_rbA.getCenterOfMassTransform();
+	//btTransform trBCur = m_rbB.getCenterOfMassTransform();
 //	btTransform trABCur = trBCur.inverse() * trACur;
 //	btQuaternion qABCur = trABCur.getRotation();
 //	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -33,7 +33,8 @@ class	btDispatcher;
 enum btConstraintSolverType
 {
 	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2
+	BT_MLCP_SOLVER=2,
+	BT_NNCG_SOLVER=4
 };
 
 class btConstraintSolver

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 		body2.getLinearVelocity(),
 		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
-	btScalar a;
-	a=jacDiagABInv;
+
 
 
 	rel_vel = normal.dot(vel);

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -89,7 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | 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 BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
+		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
 		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
@@ -111,7 +111,7 @@ struct btContactSolverInfoDoubleData
 	double		m_splitImpulseTurnErp;
 	double		m_linearSlop;
 	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;
+	double		m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
 	double		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp

@@ -21,109 +21,17 @@ subject to the following restrictions:
 
 
 btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
-:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
-{
-	m_pivotInA = frameInA.getOrigin();
-	m_pivotInB = frameInB.getOrigin();
-	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
-
-}
-
-btFixedConstraint::~btFixedConstraint ()
+:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
 {
+	setAngularLowerLimit(btVector3(0,0,0));
+	setAngularUpperLimit(btVector3(0,0,0));
+	setLinearLowerLimit(btVector3(0,0,0));
+	setLinearUpperLimit(btVector3(0,0,0));
 }
 
-	
-void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
-{
-	info->m_numConstraintRows = 6;
-	info->nub = 6;
-}
-
-void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
-{
-	//fix the 3 linear degrees of freedom
-
-	
-	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
-	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
-	
-
-	info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = worldOrnA*m_pivotInA;
-	{
-		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
-		btVector3 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;
-	}
-	
-	btVector3 a2 = worldOrnB*m_pivotInB;
-   
-	{
-	//	btVector3 a2n = -a2;
-		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
 
-    // set right hand side for the linear dofs
-	btScalar k = info->fps * info->erp;
-	
-	btVector3 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
-
-	btVector3 diff;
-	btScalar angle;
-	btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
-	btQuaternion qrelCur;
-	mrelCur.getRotation(qrelCur);
-	btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
-	diff*=-angle;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
-    }
-
-}
+btFixedConstraint::~btFixedConstraint ()
+{
+}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h

@@ -16,33 +16,17 @@ subject to the following restrictions:
 #ifndef BT_FIXED_CONSTRAINT_H
 #define BT_FIXED_CONSTRAINT_H
 
-#include "btTypedConstraint.h"
+#include "btGeneric6DofSpring2Constraint.h"
 
-ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
+
+ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
 {
-	btVector3 m_pivotInA;
-	btVector3 m_pivotInB;
-	btQuaternion m_relTargetAB;
 
 public:
 	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
-	
-	virtual ~btFixedConstraint();
 
 	
-	virtual void getInfo1 (btConstraintInfo1* info);
-
-	virtual void getInfo2 (btConstraintInfo2* info);
-
-	virtual	void	setParam(int num, btScalar value, int axis = -1)
-	{
-		btAssert(0);
-	}
-	virtual	btScalar getParam(int num, int axis = -1) const
-	{
-		btAssert(0);
-		return 0.f;
-	}
+	virtual ~btFixedConstraint();
 
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp

@@ -1,54 +1,54 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 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.
-*/
-
-/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
-
-#include "btGearConstraint.h"
-
-btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
-:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
-m_axisInA(axisInA),
-m_axisInB(axisInB),
-m_ratio(ratio)
-{
-}
-
-btGearConstraint::~btGearConstraint ()
-{
-}
-
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
-{
-	info->m_numConstraintRows = 1;
-	info->nub = 1;
-}
-
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
-{
-	btVector3 globalAxisA, globalAxisB;
-
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
-
-	info->m_J1angularAxis[0] = globalAxisA[0];
-	info->m_J1angularAxis[1] = globalAxisA[1];
-	info->m_J1angularAxis[2] = globalAxisA[2];
-
-	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
-	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
-	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
-
-}
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 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.
+*/
+
+/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
+
+#include "btGearConstraint.h"
+
+btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
+:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
+m_axisInA(axisInA),
+m_axisInB(axisInB),
+m_ratio(ratio)
+{
+}
+
+btGearConstraint::~btGearConstraint ()
+{
+}
+
+void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 1;
+	info->nub = 1;
+}
+
+void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btVector3 globalAxisA, globalAxisB;
+
+	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
+	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+
+	info->m_J1angularAxis[0] = globalAxisA[0];
+	info->m_J1angularAxis[1] = globalAxisA[1];
+	info->m_J1angularAxis[2] = globalAxisA[2];
+
+	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
+	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
+	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
+
+}
+

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -1,79 +1,79 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 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 BT_GEAR_CONSTRAINT_H
-#define BT_GEAR_CONSTRAINT_H
-
-#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
-
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define btGearConstraintData	btGearConstraintDoubleData
-#define btGearConstraintDataName	"btGearConstraintDoubleData"
-#else
-#define btGearConstraintData	btGearConstraintFloatData
-#define btGearConstraintDataName	"btGearConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
-
-///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
-///See Bullet/Demos/ConstraintDemo for an example use.
-class btGearConstraint : public btTypedConstraint
-{
-protected:
-	btVector3	m_axisInA;
-	btVector3	m_axisInB;
-	bool		m_useFrameA;
-	btScalar	m_ratio;
-
-public:
-	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
-	virtual ~btGearConstraint ();
-
-	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info);
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 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 BT_GEAR_CONSTRAINT_H
+#define BT_GEAR_CONSTRAINT_H
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGearConstraintData	btGearConstraintDoubleData
+#define btGearConstraintDataName	"btGearConstraintDoubleData"
+#else
+#define btGearConstraintData	btGearConstraintFloatData
+#define btGearConstraintDataName	"btGearConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
+///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
+///See Bullet/Demos/ConstraintDemo for an example use.
+class btGearConstraint : public btTypedConstraint
+{
+protected:
+	btVector3	m_axisInA;
+	btVector3	m_axisInB;
+	bool		m_useFrameA;
+	btScalar	m_ratio;
+
+public:
+	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
+	virtual ~btGearConstraint ();
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo1 (btConstraintInfo1* info);
+
 	///internal method used by the constraint solver, don't use them directly
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-	void setAxisA(btVector3& axisA) 
-	{
-		m_axisInA = axisA;
+	void setAxisA(btVector3& axisA) 
+	{
+		m_axisInA = axisA;
 	}
-	void setAxisB(btVector3& axisB)
-	{
-		m_axisInB = axisB;
+	void setAxisB(btVector3& axisB)
+	{
+		m_axisInB = axisB;
 	}
-	void setRatio(btScalar ratio)
-	{
-		m_ratio = ratio;
+	void setRatio(btScalar ratio)
+	{
+		m_ratio = ratio;
 	}
-	const btVector3& getAxisA() const
-	{
-		return m_axisInA;
+	const btVector3& getAxisA() const
+	{
+		return m_axisInA;
 	}
-	const btVector3& getAxisB() const
-	{
-		return m_axisInB;
+	const btVector3& getAxisB() const
+	{
+		return m_axisInB;
 	}
-	btScalar getRatio() const
-	{
-		return m_ratio;
+	btScalar getRatio() const
+	{
+		return m_ratio;
 	}
 
 
@@ -84,69 +84,69 @@ public:
 		(void) axis;
 		btAssert(0);
 	}
-
-	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const 
-	{ 
-		(void) num;
-		(void) axis;
-		btAssert(0);
-		return 0.f;
-	}
-
-	virtual	int	calculateSerializeBufferSize() const;
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-};
-
-
-
-
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct btGearConstraintFloatData
-{
-	btTypedConstraintFloatData	m_typeConstraintData;
-
-	btVector3FloatData			m_axisInA;
-	btVector3FloatData			m_axisInB;
-
-	float							m_ratio;
-	char							m_padding[4];
-};
-
-struct btGearConstraintDoubleData
-{
-	btTypedConstraintDoubleData	m_typeConstraintData;
-
-	btVector3DoubleData			m_axisInA;
-	btVector3DoubleData			m_axisInB;
-
-	double						m_ratio;
-};
-
-SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
-{
-	return sizeof(btGearConstraintData);
-}
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
-{
-	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
-
-	m_axisInA.serialize( gear->m_axisInA );
-	m_axisInB.serialize( gear->m_axisInB );
-
-	gear->m_ratio = m_ratio;
-
-	return btGearConstraintDataName;
-}
-
-
-
-
-
-
-#endif //BT_GEAR_CONSTRAINT_H
+
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const 
+	{ 
+		(void) num;
+		(void) axis;
+		btAssert(0);
+		return 0.f;
+	}
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+};
+
+
+
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGearConstraintFloatData
+{
+	btTypedConstraintFloatData	m_typeConstraintData;
+
+	btVector3FloatData			m_axisInA;
+	btVector3FloatData			m_axisInB;
+
+	float							m_ratio;
+	char							m_padding[4];
+};
+
+struct btGearConstraintDoubleData
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+
+	btVector3DoubleData			m_axisInA;
+	btVector3DoubleData			m_axisInB;
+
+	double						m_ratio;
+};
+
+SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGearConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+
+	m_axisInA.serialize( gear->m_axisInA );
+	m_axisInB.serialize( gear->m_axisInB );
+
+	gear->m_ratio = m_ratio;
+
+	return btGearConstraintDataName;
+}
+
+
+
+
+
+
+#endif //BT_GEAR_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -0,0 +1,1121 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
+Pros:
+- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
+- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
+- Servo motor functionality
+- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
+- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
+
+Cons:
+- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
+- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+
+
+#include "btGeneric6DofSpring2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+
+btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
+	, m_frameInA(frameInA)
+	, m_frameInB(frameInB)
+	, m_rotateOrder(rotOrder)	
+	, m_flags(0)
+{
+	calculateTransforms();
+}
+
+
+btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
+	, m_frameInB(frameInB)
+	, m_rotateOrder(rotOrder)
+	, m_flags(0)
+{
+	///not providing rigidbody A means implicitly using worldspace for body A
+	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateTransforms();
+}
+
+
+btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
+{
+	int i = index%3;
+	int j = index/3;
+	return mat[i][j];
+}
+
+// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz          -cy*sz           sy
+	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,2);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(btGetMatrixElem(mat,2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			return true;
+		}
+		else
+		{
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = -SIMD_HALF_PI;
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[1] = SIMD_HALF_PI;
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz          -sz           sy*cz
+	//        cy*cx*sz+sx*sy  cx*cz        sy*cx*sz-cy*sx
+	//        cy*sx*sz-cx*sy  sx*cz        sy*sx*sz+cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,1);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
+			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+			xyz[2] = btAsin(-btGetMatrixElem(mat,1));
+			return true;
+		}
+		else
+		{
+			xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+			xyz[1] = btScalar(0.0);
+			xyz[2] = SIMD_HALF_PI;
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+		xyz[1] = 0.0;
+		xyz[2] = -SIMD_HALF_PI;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz+sy*sx*sz  cz*sy*sx-cy*sz  cx*sy
+	//        cx*sz           cx*cz           -sx
+	//        cy*sx*sz-cz*sy  sy*sz+cy*cz*sx  cy*cx
+
+	btScalar fi = btGetMatrixElem(mat,5);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAsin(-btGetMatrixElem(mat,5));
+			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
+			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			return true;
+		}
+		else
+		{
+			xyz[0] = SIMD_HALF_PI;
+			xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = -SIMD_HALF_PI;
+		xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cy*cz   sy*sx-cy*cx*sz   cx*sy+cy*sz*sx
+	//        sz           cz*cx           -cz*sx
+	//        -cz*sy  cy*sx+cx*sy*sz   cy*cx-sy*sz*sx
+
+	btScalar fi = btGetMatrixElem(mat,3);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
+			xyz[2] = btAsin(btGetMatrixElem(mat,3));
+			return true;
+		}
+		else
+		{
+			xyz[0] = btScalar(0.0);
+			xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+			xyz[2] = -SIMD_HALF_PI;
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btScalar(0.0);
+		xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+		xyz[2] = SIMD_HALF_PI;
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cz*cy-sz*sx*sy    -cx*sz   cz*sy+cy*sz*sx
+	//        cy*sz+cz*sx*sy     cz*cx   sz*sy-cz*xy*sx
+	//        -cx*sy              sx     cx*cy
+
+	btScalar fi = btGetMatrixElem(mat,7);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAsin(btGetMatrixElem(mat,7));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
+			return true;
+		}
+		else
+		{
+			xyz[0] = -SIMD_HALF_PI;
+			xyz[1] = btScalar(0.0);
+			xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = SIMD_HALF_PI;
+		xyz[1] = btScalar(0.0);
+		xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+	}
+	return false;
+}
+
+bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
+{
+	// rot =  cz*cy   cz*sy*sx-cx*sz   sz*sx+cz*cx*sy
+	//        cy*sz   cz*cx+sz*sy*sx   cx*sz*sy-cz*sx
+	//        -sy          cy*sx         cy*cx
+
+	btScalar fi = btGetMatrixElem(mat,6);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(-btGetMatrixElem(mat,6));
+			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
+			return true;
+		}
+		else
+		{
+			xyz[0] = btScalar(0.0);
+			xyz[1] = SIMD_HALF_PI;
+			xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
+			return false;
+		}
+	}
+	else
+	{
+		xyz[0] = btScalar(0.0);
+		xyz[1] = -SIMD_HALF_PI;
+		xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
+	}
+	return false;
+}
+
+void btGeneric6DofSpring2Constraint::calculateAngleInfo()
+{
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+	switch (m_rotateOrder)
+	{
+		case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
+		case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
+		default : btAssert(false);
+	}
+	// 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.
+	switch (m_rotateOrder)
+	{
+	case RO_XYZ :
+		{
+			//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
+			//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
+			//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
+			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
+			// x' = Nperp = N.cross(axis2)
+			// y' = N = axis2.cross(axis0)	
+			// z' = z
+			//
+			// x" = X
+			// y" = y'
+			// z" = ??
+			//in other words:
+			//first rotate around z
+			//second rotate around y'= z.cross(X)
+			//third rotate around x" = X
+			//Original XYZ extrinsic rotation order. 
+			//Planes: xy and YZ normals: z, X.  Plane intersection (N) is z.cross(X)
+			btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+			btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+			m_calculatedAxis[1] = axis2.cross(axis0);
+			m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+			break;
+		}
+	case RO_XZY :
+		{
+			//planes: xz,ZY normals: y, X
+			//first rotate around y
+			//second rotate around z'= y.cross(X)
+			//third rotate around x" = X
+			btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+			btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
+			m_calculatedAxis[2] = axis0.cross(axis1);
+			m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
+			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
+			break;
+		}
+	case RO_YXZ :
+		{
+			//planes: yx,XZ normals: z, Y
+			//first rotate around z
+			//second rotate around x'= z.cross(Y)
+			//third rotate around y" = Y
+			btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
+			btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+			m_calculatedAxis[0] = axis1.cross(axis2);
+			m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
+			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
+			break;
+		}
+	case RO_YZX :
+		{
+			//planes: yz,ZX normals: x, Y
+			//first rotate around x
+			//second rotate around z'= x.cross(Y)
+			//third rotate around y" = Y
+			btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
+			btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
+			m_calculatedAxis[2] = axis0.cross(axis1);
+			m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
+			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
+			break;
+		}
+	case RO_ZXY :
+		{
+			//planes: zx,XY normals: y, Z
+			//first rotate around y
+			//second rotate around x'= y.cross(Z)
+			//third rotate around z" = Z
+			btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
+			btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
+			m_calculatedAxis[0] = axis1.cross(axis2);
+			m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
+			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
+			break;
+		}
+	case RO_ZYX :
+		{
+			//planes: zy,YX normals: x, Z
+			//first rotate around x
+			//second rotate around y' = x.cross(Z)
+			//third rotate around z" = Z
+			btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
+			btVector3 axis2 = m_calculatedTransformB.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]);
+			break;
+		}
+	default:
+		btAssert(false);
+	}
+
+	m_calculatedAxis[0].normalize();
+	m_calculatedAxis[1].normalize();
+	m_calculatedAxis[2].normalize();
+
+}
+
+void btGeneric6DofSpring2Constraint::calculateTransforms()
+{
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+	m_calculatedTransformA = transA * m_frameInA;
+	m_calculatedTransformB = transB * m_frameInB;
+	calculateLinearInfo();
+	calculateAngleInfo();
+
+	btScalar miA = getRigidBodyA().getInvMass();
+	btScalar miB = getRigidBodyB().getInvMass();
+	m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+	btScalar miS = miA + miB;
+	if(miS > btScalar(0.f))
+	{
+		m_factA = miB / miS;
+	}
+	else 
+	{
+		m_factA = btScalar(0.5f);
+	}
+	m_factB = btScalar(1.0f) - m_factA;
+}
+
+
+void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
+{
+	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+	m_angularLimits[axis_index].m_currentPosition = angle;
+	m_angularLimits[axis_index].testLimitValue(angle);
+}
+
+
+void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
+{
+	//prepare constraint
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	info->m_numConstraintRows = 0;
+	info->nub = 0;
+	int i;
+	//test linear limits
+	for(i = 0; i < 3; i++)
+	{
+		     if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
+		else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
+	}
+	//test angular limits
+	for (i=0;i<3 ;i++ )
+	{
+		testAngularLimitMotor(i);
+		     if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
+		else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
+	}
+}
+
+
+void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
+{
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+	const btVector3& linVelA = m_rbA.getLinearVelocity();
+	const btVector3& linVelB = m_rbB.getLinearVelocity();
+	const btVector3& angVelA = m_rbA.getAngularVelocity();
+	const btVector3& angVelB = m_rbB.getAngularVelocity();
+
+	// 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);
+}
+
+
+int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	//solve linear limits
+	btRotationalLimitMotor2 limot;
+	for (int i=0;i<3 ;i++ )
+	{
+		if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
+		{ // re-use rotational motor code
+			limot.m_bounce                 = m_linearLimits.m_bounce[i];
+			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_currentLimitErrorHi    = m_linearLimits.m_currentLimitErrorHi[i];
+			limot.m_enableMotor            = m_linearLimits.m_enableMotor[i];
+			limot.m_servoMotor             = m_linearLimits.m_servoMotor[i];
+			limot.m_servoTarget            = m_linearLimits.m_servoTarget[i];
+			limot.m_enableSpring           = m_linearLimits.m_enableSpring[i];
+			limot.m_springStiffness        = m_linearLimits.m_springStiffness[i];
+			limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
+			limot.m_springDamping          = m_linearLimits.m_springDamping[i];
+			limot.m_springDampingLimited   = m_linearLimits.m_springDampingLimited[i];
+			limot.m_equilibriumPoint       = m_linearLimits.m_equilibriumPoint[i];
+			limot.m_hiLimit                = m_linearLimits.m_upperLimit[i];
+			limot.m_loLimit                = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxMotorForce          = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity         = m_linearLimits.m_targetVelocity[i];
+			btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+			int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
+			limot.m_stopCFM  = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP  = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
+			limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
+			limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
+
+			//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
+			int indx1 = (i + 1) % 3;
+			int indx2 = (i + 2) % 3;
+			int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
+			#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
+			bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
+				m_angularLimits[indx1].m_currentLimit == 2 ||
+				( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
+				( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+			bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
+				m_angularLimits[indx2].m_currentLimit == 2 ||
+				( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
+				( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+			if( indx1Violated && indx2Violated )
+			{
+				rotAllowed = 0;
+			}
+			row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+
+		}
+	}
+	return row;
+}
+
+
+
+int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	int row = row_offset;
+
+	//order of rotational constraint rows
+	int cIdx[] = {0, 1, 2};
+	switch(m_rotateOrder)
+	{
+		case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
+		case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
+		case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
+		case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
+		case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
+		case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
+		default : btAssert(false);
+	}
+
+	for (int ii = 0; ii < 3 ; ii++ )
+	{
+		int i = cIdx[ii];
+		if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
+		{
+			btVector3 axis = getAxis(i);
+			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
+			if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
+			{
+				m_angularLimits[i].m_stopCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
+			{
+				m_angularLimits[i].m_stopERP = info->erp;
+			}
+			if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
+			{
+				m_angularLimits[i].m_motorCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
+			{
+				m_angularLimits[i].m_motorERP = info->erp;
+			}
+			row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+		}
+	}
+
+	return row;
+}
+
+
+void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
+{
+	m_frameInA = frameA;
+	m_frameInB = frameB;
+	buildJacobian();
+	calculateTransforms();
+}
+
+
+void btGeneric6DofSpring2Constraint::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]);
+	}
+}
+
+void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
+{
+	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+	btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+
+	J1[srow+0] = ax1[0];
+	J1[srow+1] = ax1[1];
+	J1[srow+2] = ax1[2];
+
+	J2[srow+0] = -ax1[0];
+	J2[srow+1] = -ax1[1];
+	J2[srow+2] = -ax1[2];
+
+	if(!rotational)
+	{
+		btVector3 tmpA, tmpB, relA, relB;
+		// get vector from bodyB to frameB in WCS
+		relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+		// same for bodyA
+		relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+		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];
+	}
+}
+
+
+int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
+	btRotationalLimitMotor2 * limot,
+	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+{
+	int count = 0;
+	int srow = row * info->rowskip;
+
+	if (limot->m_currentLimit==4) 
+	{
+		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
+
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
+		if (rotational) {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		} else {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		}
+		info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
+		info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
+		if (rotational) {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		} else {
+			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
+				btScalar bounceerror = -limot->m_bounce* vel;
+				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
+			}
+		}
+		info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
+		info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+	} else
+	if (limot->m_currentLimit==3) 
+	{
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->cfm[srow] = limot->m_stopCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableMotor && !limot->m_servoMotor)
+	{
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+		btScalar mot_fact = getMotorFactor(limot->m_currentPosition, 
+			limot->m_loLimit,
+			limot->m_hiLimit,
+			tag_vel,
+			info->fps * limot->m_motorERP);
+		info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->cfm[srow] = limot->m_motorCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableMotor && limot->m_servoMotor)
+	{
+		btScalar error = limot->m_currentPosition - limot->m_servoTarget;
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
+		btScalar tag_vel = -targetvelocity;
+		btScalar mot_fact;
+		if(error != 0)
+		{
+			btScalar lowLimit;
+			btScalar hiLimit;
+			if(limot->m_loLimit > limot->m_hiLimit)
+			{
+				lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
+				hiLimit  = error < 0 ? limot->m_servoTarget :  SIMD_INFINITY;
+			}
+			else
+			{
+				lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
+				hiLimit  = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
+			}
+			mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
+		} 
+		else 
+		{
+			mot_fact = 0;
+		}
+		info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->cfm[srow] = limot->m_motorCFM;
+		srow += info->rowskip;
+		++count;
+	}
+
+	if (limot->m_enableSpring)
+	{
+		btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
+		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+
+		//btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
+		//if(cfm > 0.99999)
+		//	cfm = 0.99999;
+		//btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
+		//info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
+		//info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		//info->m_upperLimit[srow] = SIMD_INFINITY;
+
+		btScalar dt = BT_ONE / info->fps;
+		btScalar kd = limot->m_springDamping;
+		btScalar ks = limot->m_springStiffness;
+		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
+//		btScalar erp = 0.1;
+		btScalar cfm = BT_ZERO;
+		btScalar mA = BT_ONE / m_rbA.getInvMass();
+		btScalar mB = BT_ONE / m_rbB.getInvMass();
+		btScalar m = mA > mB ? mB : mA;
+		btScalar angularfreq = sqrt(ks / m);
+
+
+		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
+		if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
+		{
+			ks = BT_ONE / dt / dt / btScalar(16.0) * m;
+		}
+		//avoid damping that would blow up the spring
+		if(limot->m_springDampingLimited && kd * dt > m)
+		{
+			kd = m / dt;
+		}
+		btScalar fs = ks * error * dt;
+		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
+		btScalar f = (fs+fd);
+
+		info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
+
+		btScalar minf = f < fd ? f : fd;
+		btScalar maxf = f < fd ? fd : f;
+		if(!rotational)
+		{
+			info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
+			info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
+		}
+		else
+		{
+			info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
+			info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
+		}
+
+		info->cfm[srow] = cfm;
+		srow += info->rowskip;
+		++count;
+	}
+
+	return count;
+}
+
+
+//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 btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
+{
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_linearLimits.m_stopERP[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_linearLimits.m_stopCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_ERP : 
+				m_linearLimits.m_motorERP[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_linearLimits.m_motorCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_angularLimits[axis - 3].m_stopERP = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_angularLimits[axis - 3].m_stopCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_ERP : 
+				m_angularLimits[axis - 3].m_motorERP = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_angularLimits[axis - 3].m_motorCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+}
+
+//return the local value of parameter
+btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const 
+{
+	btScalar retVal = 0;
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_stopERP[axis];
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_stopCFM[axis];
+				break;
+			case BT_CONSTRAINT_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_motorERP[axis];
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_linearLimits.m_motorCFM[axis];
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_stopERP;
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_stopCFM;
+				break;
+			case BT_CONSTRAINT_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_motorERP;
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
+				retVal = m_angularLimits[axis - 3].m_motorCFM;
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+	return retVal;
+}
+
+ 
+
+void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+{
+	btVector3 zAxis = axis1.normalized();
+	btVector3 yAxis = axis2.normalized();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	
+	btTransform 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 = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
+	
+	calculateTransforms();
+}
+
+void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_bounce[index] = bounce;
+	else
+		m_angularLimits[index - 3].m_bounce = bounce;
+}
+
+void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_enableMotor[index] = onOff;
+	else
+		m_angularLimits[index - 3].m_enableMotor = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_servoMotor[index] = onOff;
+	else
+		m_angularLimits[index - 3].m_servoMotor = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_targetVelocity[index] = velocity;
+	else
+		m_angularLimits[index - 3].m_targetVelocity = velocity;
+}
+
+void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_servoTarget[index] = target;
+	else
+		m_angularLimits[index - 3].m_servoTarget = target;
+}
+
+void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_maxMotorForce[index] = force;
+	else
+		m_angularLimits[index - 3].m_maxMotorForce = force;
+}
+
+void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_enableSpring[index] = onOff;
+	else
+		m_angularLimits[index - 3] .m_enableSpring = onOff;
+}
+
+void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3) {
+		m_linearLimits.m_springStiffness[index] = stiffness;
+		m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
+	} else {
+		m_angularLimits[index - 3].m_springStiffness = stiffness;
+		m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
+	}
+}
+
+void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3) {
+		m_linearLimits.m_springDamping[index] = damping;
+		m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
+	} else {
+		m_angularLimits[index - 3].m_springDamping = damping;
+		m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
+	}
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
+{
+	calculateTransforms();
+	int i;
+	for( i = 0; i < 3; i++)
+		m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+	for(i = 0; i < 3; i++)
+		m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
+{
+	btAssert((index >= 0) && (index < 6));
+	calculateTransforms();
+	if (index<3)
+		m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+	else
+		m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
+}
+
+void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index<3)
+		m_linearLimits.m_equilibriumPoint[index] = val;
+	else
+		m_angularLimits[index - 3] .m_equilibriumPoint = val;
+}
+
+
+//////////////////////////// btRotationalLimitMotor2 ////////////////////////////////////
+
+void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
+{
+	//we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
+	if(m_loLimit > m_hiLimit) {
+		m_currentLimit = 0;
+		m_currentLimitError = btScalar(0.f);
+	}
+	else if(m_loLimit == m_hiLimit) {
+		m_currentLimitError = test_value - m_loLimit;
+		m_currentLimit = 3;
+	} else {
+		m_currentLimitError = test_value - m_loLimit;
+		m_currentLimitErrorHi = test_value - m_hiLimit;
+		m_currentLimit = 4;
+	}
+}
+
+//////////////////////////// btTranslationalLimitMotor2 ////////////////////////////////////
+
+void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value)
+{
+	btScalar loLimit = m_lowerLimit[limitIndex];
+	btScalar hiLimit = m_upperLimit[limitIndex];
+	if(loLimit > hiLimit) {
+		m_currentLimitError[limitIndex] = 0;
+		m_currentLimit[limitIndex] = 0;
+	}
+	else if(loLimit == hiLimit) {
+		m_currentLimitError[limitIndex] = test_value - loLimit;
+		m_currentLimit[limitIndex] = 3;
+	} else {
+		m_currentLimitError[limitIndex] = test_value - loLimit;
+		m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
+		m_currentLimit[limitIndex] = 4;
+	}
+}
+
+

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -0,0 +1,674 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/*
+2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
+Pros:
+- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
+- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
+- Servo motor functionality
+- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
+- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
+
+Cons:
+- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
+- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+
+#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
+#define BT_GENERIC_6DOF_CONSTRAINT2_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
+#else
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+enum RotateOrder
+{
+	RO_XYZ=0,
+	RO_XZY,
+	RO_YXZ,
+	RO_YZX,
+	RO_ZXY,
+	RO_ZYX
+};
+
+class btRotationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btScalar m_loLimit;
+	btScalar m_hiLimit;
+	btScalar m_bounce;
+	btScalar m_stopERP;
+	btScalar m_stopCFM;
+	btScalar m_motorERP;
+	btScalar m_motorCFM;
+	bool     m_enableMotor;
+	btScalar m_targetVelocity;
+	btScalar m_maxMotorForce;
+	bool     m_servoMotor;
+	btScalar m_servoTarget;
+	bool     m_enableSpring;
+	btScalar m_springStiffness;
+	bool     m_springStiffnessLimited;
+	btScalar m_springDamping;
+	bool     m_springDampingLimited;
+	btScalar m_equilibriumPoint;
+
+	btScalar m_currentLimitError;
+	btScalar m_currentLimitErrorHi;
+	btScalar m_currentPosition;
+	int      m_currentLimit;
+
+	btRotationalLimitMotor2()
+	{
+		m_loLimit                = 1.0f;
+		m_hiLimit                = -1.0f;
+		m_bounce                 = 0.0f;
+		m_stopERP                = 0.2f;
+		m_stopCFM                = 0.f;
+		m_motorERP               = 0.9f;
+		m_motorCFM               = 0.f;
+		m_enableMotor            = false;
+		m_targetVelocity         = 0;
+		m_maxMotorForce          = 0.1f;
+		m_servoMotor             = false;
+		m_servoTarget            = 0;
+		m_enableSpring           = false;
+		m_springStiffness        = 0;
+		m_springStiffnessLimited = false;
+		m_springDamping          = 0;
+		m_springDampingLimited   = false;
+		m_equilibriumPoint       = 0;
+
+		m_currentLimitError   = 0;
+		m_currentLimitErrorHi = 0;
+		m_currentPosition     = 0;
+		m_currentLimit        = 0;
+	}
+
+	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
+	{
+		m_loLimit                = limot.m_loLimit;
+		m_hiLimit                = limot.m_hiLimit;
+		m_bounce                 = limot.m_bounce;
+		m_stopERP                = limot.m_stopERP;
+		m_stopCFM                = limot.m_stopCFM;
+		m_motorERP               = limot.m_motorERP;
+		m_motorCFM               = limot.m_motorCFM;
+		m_enableMotor            = limot.m_enableMotor;
+		m_targetVelocity         = limot.m_targetVelocity;
+		m_maxMotorForce          = limot.m_maxMotorForce;
+		m_servoMotor             = limot.m_servoMotor;
+		m_servoTarget            = limot.m_servoTarget;
+		m_enableSpring           = limot.m_enableSpring;
+		m_springStiffness        = limot.m_springStiffness;
+		m_springStiffnessLimited = limot.m_springStiffnessLimited;
+		m_springDamping          = limot.m_springDamping;
+		m_springDampingLimited   = limot.m_springDampingLimited;
+		m_equilibriumPoint       = limot.m_equilibriumPoint;
+
+		m_currentLimitError   = limot.m_currentLimitError;
+		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
+		m_currentPosition     = limot.m_currentPosition;
+		m_currentLimit        = limot.m_currentLimit;
+	}
+
+
+	bool isLimited()
+	{
+		if(m_loLimit > m_hiLimit) return false;
+		return true;
+	}
+
+	void testLimitValue(btScalar test_value);
+};
+
+
+
+class btTranslationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btVector3 m_lowerLimit;
+	btVector3 m_upperLimit;
+	btVector3 m_bounce;
+	btVector3 m_stopERP;
+	btVector3 m_stopCFM;
+	btVector3 m_motorERP;
+	btVector3 m_motorCFM;
+	bool      m_enableMotor[3];
+	bool      m_servoMotor[3];
+	bool      m_enableSpring[3];
+	btVector3 m_servoTarget;
+	btVector3 m_springStiffness;
+	bool      m_springStiffnessLimited[3];
+	btVector3 m_springDamping;
+	bool      m_springDampingLimited[3];
+	btVector3 m_equilibriumPoint;
+	btVector3 m_targetVelocity;
+	btVector3 m_maxMotorForce;
+
+	btVector3 m_currentLimitError;
+	btVector3 m_currentLimitErrorHi;
+	btVector3 m_currentLinearDiff;
+	int       m_currentLimit[3];
+
+	btTranslationalLimitMotor2()
+	{
+		m_lowerLimit         .setValue(0.f , 0.f , 0.f );
+		m_upperLimit         .setValue(0.f , 0.f , 0.f );
+		m_bounce             .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_motorERP           .setValue(0.9f, 0.9f, 0.9f);
+		m_motorCFM           .setValue(0.f , 0.f , 0.f );
+
+		m_currentLimitError  .setValue(0.f , 0.f , 0.f );
+		m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
+		m_currentLinearDiff  .setValue(0.f , 0.f , 0.f );
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]            = false;
+			m_servoMotor[i]             = false;
+			m_enableSpring[i]           = false;
+			m_servoTarget[i]            = btScalar(0.f);
+			m_springStiffness[i]        = btScalar(0.f);
+			m_springStiffnessLimited[i] = false;
+			m_springDamping[i]          = btScalar(0.f);
+			m_springDampingLimited[i]   = false;
+			m_equilibriumPoint[i]       = btScalar(0.f);
+			m_targetVelocity[i]         = btScalar(0.f);
+			m_maxMotorForce[i]          = btScalar(0.f);
+			
+			m_currentLimit[i]     = 0;
+		}
+	}
+
+	btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
+	{
+		m_lowerLimit          = other.m_lowerLimit;
+		m_upperLimit          = other.m_upperLimit;
+		m_bounce              = other.m_bounce;
+		m_stopERP             = other.m_stopERP;
+		m_stopCFM             = other.m_stopCFM;
+		m_motorERP            = other.m_motorERP;
+		m_motorCFM            = other.m_motorCFM;
+		
+		m_currentLimitError   = other.m_currentLimitError;
+		m_currentLimitErrorHi = other.m_currentLimitErrorHi;
+		m_currentLinearDiff   = other.m_currentLinearDiff;
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]            = other.m_enableMotor[i];
+			m_servoMotor[i]             = other.m_servoMotor[i];
+			m_enableSpring[i]           = other.m_enableSpring[i];
+			m_servoTarget[i]            = other.m_servoTarget[i];
+			m_springStiffness[i]        = other.m_springStiffness[i];
+			m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
+			m_springDamping[i]          = other.m_springDamping[i];
+			m_springDampingLimited[i]   = other.m_springDampingLimited[i];
+			m_equilibriumPoint[i]       = other.m_equilibriumPoint[i];
+			m_targetVelocity[i]         = other.m_targetVelocity[i];
+			m_maxMotorForce[i]          = other.m_maxMotorForce[i];
+
+			m_currentLimit[i]     = other.m_currentLimit[i];
+		}
+	}
+
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+
+	void testLimitValue(int limitIndex, btScalar test_value);
+};
+
+enum bt6DofFlags2
+{
+	BT_6DOF_FLAGS_CFM_STOP2 = 1,
+	BT_6DOF_FLAGS_ERP_STOP2 = 2,
+	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
+
+
+ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
+{
+protected:
+
+	btTransform m_frameInA;
+	btTransform m_frameInB;
+
+	btJacobianEntry m_jacLinear[3];
+	btJacobianEntry m_jacAng[3];
+
+	btTranslationalLimitMotor2 m_linearLimits;
+	btRotationalLimitMotor2 m_angularLimits[3];
+
+	RotateOrder m_rotateOrder;
+
+protected:
+
+	btTransform  m_calculatedTransformA;
+	btTransform  m_calculatedTransformB;
+	btVector3    m_calculatedAxisAngleDiff;
+	btVector3    m_calculatedAxis[3];
+	btVector3    m_calculatedLinearDiff;
+	btScalar     m_factA;
+	btScalar     m_factB;
+	bool         m_hasStaticBody;
+	int          m_flags;
+
+	btGeneric6DofSpring2Constraint&	operator=(btGeneric6DofSpring2Constraint&)
+	{
+		btAssert(0);
+		return *this;
+	}
+
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+	void calculateLinearInfo();
+	void calculateAngleInfo();
+	void testAngularLimitMotor(int axis_index);
+
+	void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
+	int get_limit_motor_info2(btRotationalLimitMotor2* limot,
+		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+
+	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+	static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+    btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+
+	virtual void buildJacobian() {}
+	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual int calculateSerializeBufferSize() const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
+	btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
+
+	// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
+	void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms();
+
+	// Gets the global transform of the offset for body A
+	const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+	// Gets the global transform of the offset for body B
+	const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+
+	const btTransform & getFrameOffsetA() const { return m_frameInA; }
+	const btTransform & getFrameOffsetB() const { return m_frameInB; }
+
+	btTransform & getFrameOffsetA() { return m_frameInA; }
+	btTransform & getFrameOffsetB() { return m_frameInB; }
+
+	// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
+
+	// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
+
+	// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
+
+	void setFrames(const btTransform & frameA, const btTransform & frameB);
+
+	void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
+	void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
+	void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
+	void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
+
+	void setAngularLowerLimit(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+	}
+
+	void setAngularLowerLimitReversed(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
+	}
+
+	void getAngularLowerLimit(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			angularLower[i] = m_angularLimits[i].m_loLimit;
+	}
+
+	void getAngularLowerLimitReversed(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++)
+			angularLower[i] = -m_angularLimits[i].m_hiLimit;
+	}
+
+	void setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+	}
+
+	void setAngularUpperLimitReversed(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
+	}
+
+	void getAngularUpperLimit(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = m_angularLimits[i].m_hiLimit;
+	}
+
+	void getAngularUpperLimitReversed(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = -m_angularLimits[i].m_loLimit;
+	}
+
+	//first 3 are linear, next 3 are angular
+
+	void setLimit(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_loLimit = lo;
+			m_angularLimits[axis-3].m_hiLimit = hi;
+		}
+	}
+
+	void setLimitReversed(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_hiLimit = -lo;
+			m_angularLimits[axis-3].m_loLimit = -hi;
+		}
+	}
+
+	bool isLimited(int limitIndex)
+	{
+		if(limitIndex<3)
+		{
+			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex-3].isLimited();
+	}
+
+	void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
+	RotateOrder getRotationOrder() { return m_rotateOrder; }
+
+	void setAxis( const btVector3& axis1, const btVector3& axis2);
+
+	void setBounce(int index, btScalar bounce);
+
+	void enableMotor(int index, bool onOff);
+	void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
+	void setTargetVelocity(int index, btScalar velocity);
+	void setServoTarget(int index, btScalar target);
+	void setMaxMotorForce(int index, btScalar force);
+
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
+	void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	void setEquilibriumPoint(int index, btScalar val);
+
+	//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, btScalar value, int axis = -1);
+	virtual btScalar getParam(int num, int axis = -1) const;
+};
+
+
+struct btGeneric6DofSpring2ConstraintData
+{
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;
+	btTransformFloatData m_rbBFrame;
+
+	btVector3FloatData m_linearUpperLimit;
+	btVector3FloatData m_linearLowerLimit;
+	btVector3FloatData m_linearBounce;
+	btVector3FloatData m_linearStopERP;
+	btVector3FloatData m_linearStopCFM;
+	btVector3FloatData m_linearMotorERP;
+	btVector3FloatData m_linearMotorCFM;
+	btVector3FloatData m_linearTargetVelocity;
+	btVector3FloatData m_linearMaxMotorForce;
+	btVector3FloatData m_linearServoTarget;
+	btVector3FloatData m_linearSpringStiffness;
+	btVector3FloatData m_linearSpringDamping;
+	btVector3FloatData m_linearEquilibriumPoint;
+	char               m_linearEnableMotor[4];
+	char               m_linearServoMotor[4];
+	char               m_linearEnableSpring[4];
+	char               m_linearSpringStiffnessLimited[4];
+	char               m_linearSpringDampingLimited[4];
+	char               m_padding1[4];
+
+	btVector3FloatData m_angularUpperLimit;
+	btVector3FloatData m_angularLowerLimit;
+	btVector3FloatData m_angularBounce;
+	btVector3FloatData m_angularStopERP;
+	btVector3FloatData m_angularStopCFM;
+	btVector3FloatData m_angularMotorERP;
+	btVector3FloatData m_angularMotorCFM;
+	btVector3FloatData m_angularTargetVelocity;
+	btVector3FloatData m_angularMaxMotorForce;
+	btVector3FloatData m_angularServoTarget;
+	btVector3FloatData m_angularSpringStiffness;
+	btVector3FloatData m_angularSpringDamping;
+	btVector3FloatData m_angularEquilibriumPoint;
+	char               m_angularEnableMotor[4];
+	char               m_angularServoMotor[4];
+	char               m_angularEnableSpring[4];
+	char               m_angularSpringStiffnessLimited[4];
+	char               m_angularSpringDampingLimited[4];
+
+	int                m_rotateOrder;
+};
+
+struct btGeneric6DofSpring2ConstraintDoubleData2
+{
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;
+	btTransformDoubleData m_rbBFrame;
+
+	btVector3DoubleData m_linearUpperLimit;
+	btVector3DoubleData m_linearLowerLimit;
+	btVector3DoubleData m_linearBounce;
+	btVector3DoubleData m_linearStopERP;
+	btVector3DoubleData m_linearStopCFM;
+	btVector3DoubleData m_linearMotorERP;
+	btVector3DoubleData m_linearMotorCFM;
+	btVector3DoubleData m_linearTargetVelocity;
+	btVector3DoubleData m_linearMaxMotorForce;
+	btVector3DoubleData m_linearServoTarget;
+	btVector3DoubleData m_linearSpringStiffness;
+	btVector3DoubleData m_linearSpringDamping;
+	btVector3DoubleData m_linearEquilibriumPoint;
+	char                m_linearEnableMotor[4];
+	char                m_linearServoMotor[4];
+	char                m_linearEnableSpring[4];
+	char                m_linearSpringStiffnessLimited[4];
+	char                m_linearSpringDampingLimited[4];
+	char                m_padding1[4];
+
+	btVector3DoubleData m_angularUpperLimit;
+	btVector3DoubleData m_angularLowerLimit;
+	btVector3DoubleData m_angularBounce;
+	btVector3DoubleData m_angularStopERP;
+	btVector3DoubleData m_angularStopCFM;
+	btVector3DoubleData m_angularMotorERP;
+	btVector3DoubleData m_angularMotorCFM;
+	btVector3DoubleData m_angularTargetVelocity;
+	btVector3DoubleData m_angularMaxMotorForce;
+	btVector3DoubleData m_angularServoTarget;
+	btVector3DoubleData m_angularSpringStiffness;
+	btVector3DoubleData m_angularSpringDamping;
+	btVector3DoubleData m_angularEquilibriumPoint;
+	char                m_angularEnableMotor[4];
+	char                m_angularServoMotor[4];
+	char                m_angularEnableSpring[4];
+	char                m_angularSpringStiffnessLimited[4];
+	char                m_angularSpringDampingLimited[4];
+
+	int                 m_rotateOrder;
+};
+
+SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGeneric6DofSpring2ConstraintData2);
+}
+
+SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+
+	m_frameInA.serialize(dof->m_rbAFrame);
+	m_frameInB.serialize(dof->m_rbBFrame);
+
+	int i;
+	for (i=0;i<3;i++)
+	{
+		dof->m_angularLowerLimit.m_floats[i]       = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i]       = m_angularLimits[i].m_hiLimit;
+		dof->m_angularBounce.m_floats[i]           = m_angularLimits[i].m_bounce;
+		dof->m_angularStopERP.m_floats[i]          = m_angularLimits[i].m_stopERP;
+		dof->m_angularStopCFM.m_floats[i]          = m_angularLimits[i].m_stopCFM;
+		dof->m_angularMotorERP.m_floats[i]         = m_angularLimits[i].m_motorERP;
+		dof->m_angularMotorCFM.m_floats[i]         = m_angularLimits[i].m_motorCFM;
+		dof->m_angularTargetVelocity.m_floats[i]   = m_angularLimits[i].m_targetVelocity;
+		dof->m_angularMaxMotorForce.m_floats[i]    = m_angularLimits[i].m_maxMotorForce;
+		dof->m_angularServoTarget.m_floats[i]      = m_angularLimits[i].m_servoTarget;
+		dof->m_angularSpringStiffness.m_floats[i]  = m_angularLimits[i].m_springStiffness;
+		dof->m_angularSpringDamping.m_floats[i]    = m_angularLimits[i].m_springDamping;
+		dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
+	}
+	dof->m_angularLowerLimit.m_floats[3]       = 0;
+	dof->m_angularUpperLimit.m_floats[3]       = 0;
+	dof->m_angularBounce.m_floats[3]           = 0;
+	dof->m_angularStopERP.m_floats[3]          = 0;
+	dof->m_angularStopCFM.m_floats[3]          = 0;
+	dof->m_angularMotorERP.m_floats[3]         = 0;
+	dof->m_angularMotorCFM.m_floats[3]         = 0;
+	dof->m_angularTargetVelocity.m_floats[3]   = 0;
+	dof->m_angularMaxMotorForce.m_floats[3]    = 0;
+	dof->m_angularServoTarget.m_floats[3]      = 0;
+	dof->m_angularSpringStiffness.m_floats[3]  = 0;
+	dof->m_angularSpringDamping.m_floats[3]    = 0;
+	dof->m_angularEquilibriumPoint.m_floats[3] = 0;
+	for (i=0;i<4;i++)
+	{
+		dof->m_angularEnableMotor[i]            = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
+		dof->m_angularServoMotor[i]             = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
+		dof->m_angularEnableSpring[i]           = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
+		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
+		dof->m_angularSpringDampingLimited[i]   = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
+	}
+
+	m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
+	m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
+	m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
+	m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
+	m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
+	m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
+	m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
+	m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
+	m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
+	m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
+	m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
+	m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
+	m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
+	for (i=0;i<4;i++)
+	{
+		dof->m_linearEnableMotor[i]            = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearServoMotor[i]             = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearEnableSpring[i]           = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
+		dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
+		dof->m_linearSpringDampingLimited[i]   = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
+	}
+
+	dof->m_rotateOrder = m_rotateOrder;
+
+	return btGeneric6DofSpring2ConstraintDataName;
+}
+
+
+
+
+
+#endif //BT_GENERIC_6DOF_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp

@@ -25,7 +25,7 @@ subject to the following restrictions:
 // anchor, axis1 and axis2 are in world coordinate system
 // axis1 must be orthogonal to axis2
 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
-: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
  m_anchor(anchor),
  m_axis1(axis1),
  m_axis2(axis2)
@@ -59,7 +59,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec
 	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
 	// enable suspension
 	enableSpring(2, true);
-	setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
+	setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
 	setDamping(2, 0.01f);
 	setEquilibriumPoint();
 }

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h

@@ -20,7 +20,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
-#include "btGeneric6DofSpringConstraint.h"
+#include "btGeneric6DofSpring2Constraint.h"
 
 
 
@@ -29,7 +29,7 @@ subject to the following restrictions:
 // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
 // 1 translational (along axis Z) with suspension spring
 
-ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint
+ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
 {
 protected:
 	btVector3	m_anchor;

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 									 m_useReferenceFrameA(useReferenceFrameA),
-									 m_flags(0)
+									 m_flags(0),
+									 m_normalCFM(0),
+									 m_normalERP(0),
+									 m_stopCFM(0),
+									 m_stopERP(0)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
 	
@@ -101,7 +105,11 @@ m_angularOnly(false), m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -151,7 +159,11 @@ m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 #ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
@@ -177,7 +189,11 @@ m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0)
+m_flags(0),
+m_normalCFM(0),
+m_normalERP(0),
+m_stopCFM(0),
+m_stopERP(0)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
@@ -285,8 +301,60 @@ void	btHingeConstraint::buildJacobian()
 #endif //__SPU__
 
 
+static inline btScalar btNormalizeAnglePositive(btScalar angle)
+{
+  return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
+}
+
+
+
+static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
+{
+	btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
+	btNormalizeAnglePositive(accAngle)));
+	return result;
+}
+
+static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
+{
+	btScalar tol(0.3);
+	btScalar result = btShortestAngularDistance(accAngle, curAngle);
+
+	  if (btFabs(result) > tol)
+		return curAngle;
+	  else
+		return accAngle + result;
+
+	return curAngle;
+}
+
+
+btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
+{
+	btScalar hingeAngle = getHingeAngle();
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
+	return m_accumulatedAngle;
+}
+void	btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
+{
+	m_accumulatedAngle  = accAngle;
+}
+
+void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
+{
+	//update m_accumulatedAngle
+	btScalar curHingeAngle = getHingeAngle();
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
+
+	btHingeConstraint::getInfo1(info);
+	
+}
+
+
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
+
+
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
@@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
 	// linear RHS
-    btScalar k = info->fps * info->erp;
+	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
+
+    btScalar k = info->fps * normalErp;
 	if (!m_angularOnly)
 	{
 		for(i = 0; i < 3; i++)
@@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 			powered = 0;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
-		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 		if(powered)
 		{
 			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -606,6 +676,8 @@ void	btHingeConstraint::updateRHS(btScalar	timeStep)
 }
 
 
+
+
 btScalar btHingeConstraint::getHingeAngle()
 {
 	return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
@@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
     for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
 
-	btScalar k = info->fps * info->erp;
+	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
+	btScalar k = info->fps * normalErp;
 
 	if (!m_angularOnly)
 	{
@@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
 	// ax1 x ax2 is in the plane space of ax1, so we project the angular
 	// velocity to p and q to find the right hand side.
-	k = info->fps * info->erp;
+	k = info->fps * normalErp;//??
+
 	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[s3] = k * u.dot(p);
 	info->m_constraintError[s4] = k * u.dot(q);
@@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 			powered = 0;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
-		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
 		if(powered)
 		{
 			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
@@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 				m_normalCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_NORM;
 				break;
+			case BT_CONSTRAINT_ERP:
+				m_normalERP = value;
+				m_flags |= BT_HINGE_FLAGS_ERP_NORM;
+				break;
 			default : 
 				btAssertConstrParams(0);
 		}
@@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
 				retVal = m_normalCFM;
 				break;
+			case BT_CONSTRAINT_ERP:
+				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
+				retVal = m_normalERP;
+				break;
 			default : 
 				btAssertConstrParams(0);
 		}

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -41,7 +41,8 @@ enum btHingeFlags
 {
 	BT_HINGE_FLAGS_CFM_STOP = 1,
 	BT_HINGE_FLAGS_ERP_STOP = 2,
-	BT_HINGE_FLAGS_CFM_NORM = 4
+	BT_HINGE_FLAGS_CFM_NORM = 4,
+	BT_HINGE_FLAGS_ERP_NORM = 8
 };
 
 
@@ -94,6 +95,7 @@ public:
 
 	int			m_flags;
 	btScalar	m_normalCFM;
+	btScalar	m_normalERP;
 	btScalar	m_stopCFM;
 	btScalar	m_stopERP;
 
@@ -217,6 +219,14 @@ public:
 
 	}
 
+    bool hasLimit() const {
+#ifdef  _BT_USE_CENTER_LIMIT_
+        return m_limit.getHalfRange() > 0;
+#else
+        return m_lowerLimit <= m_upperLimit;
+#endif
+    }
+
 	btScalar	getLowerLimit() const
 	{
 #ifdef	_BT_USE_CENTER_LIMIT_
@@ -236,6 +246,7 @@ public:
 	}
 
 
+	///The getHingeAngle gives the hinge angle in range [-PI,PI]
 	btScalar getHingeAngle();
 
 	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
@@ -326,6 +337,43 @@ struct	btHingeConstraintDoubleData
 };
 #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
+///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
+ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
+{
+protected:
+	btScalar	m_accumulatedAngle;
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+	
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+
+	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
+	:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
+	{
+		m_accumulatedAngle=getHingeAngle();
+	}
+	btScalar getAccumulatedHingeAngle();
+	void	setAccumulatedHingeAngle(btScalar accAngle);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+};
 
 struct	btHingeConstraintFloatData
 {

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp

@@ -0,0 +1,463 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 "btNNCGConstraintSolver.h"
+
+
+
+
+
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+
+	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	return val;
+}
+
+btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+{
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	if (infoGlobal.m_solverMode & 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 = btRandInt2(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 = btRandInt2(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 = btRandInt2(j+1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+
+	btScalar deltaflengthsqr = 0;
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	} else 
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	}
+
+
+	if (m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+		} else {
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+			} else 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) 
+					{
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+
+			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					btScalar totalImpulse =0;
+
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						m_deltafC[c] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier] = 0;
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier+1] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier+1] = 0;
+							}
+						}
+					}
+				}
+
+			}
+			else//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 btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafC[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				}
+
+
+
+				///solve all friction constraints, using SIMD, if available
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(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);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						m_deltafCF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCF[j] = 0;
+					}
+				}
+
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						m_deltafCRF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCRF[j] = 0;
+					}
+				}
+
+
+			}
+		}
+
+
+
+	} else
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			for (int j=0;j<numPoolConstraints;j++)
+			{
+				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				m_deltafC[j] = deltaf;
+				deltaflengthsqr += deltaf*deltaf;
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			for (int j=0;j<numFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+				if (totalImpulse>btScalar(0))
+				{
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafCF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCF[j] = 0;
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse>btScalar(0))
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					m_deltafCRF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCRF[j] = 0;
+				}
+			}
+		}
+	}
+
+
+
+
+	if (!m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
+			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
+			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
+			{
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+			}
+		} else 
+		{
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
+				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
+				}
+			} else {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pCF[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
+					{
+						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+						if (iteration< infoGlobal.m_numIterations) {
+							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
+							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
+							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+							const btSolverConstraint& c = constraint;
+							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						}
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+	return deltaflengthsqr;
+}
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	m_pNC.resizeNoInitialize(0);
+	m_pC.resizeNoInitialize(0);
+	m_pCF.resizeNoInitialize(0);
+	m_pCRF.resizeNoInitialize(0);
+
+	m_deltafNC.resizeNoInitialize(0);
+	m_deltafC.resizeNoInitialize(0);
+	m_deltafCF.resizeNoInitialize(0);
+	m_deltafCRF.resizeNoInitialize(0);
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
+}
+
+
+

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h

@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
+#define BT_NNCG_CONSTRAINT_SOLVER_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+
+ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+protected:
+
+	btScalar m_deltafLengthSqrPrev;
+
+	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
+
+	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
+	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+
+		
+protected:
+
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_NNCG_SOLVER;
+	}
+
+	bool m_onlyForNoneContact;
+};
+
+
+
+
+#endif //BT_NNCG_CONSTRAINT_SOLVER_H
+

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -24,6 +24,8 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
 #include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btCpuFeatureUtility.h"
+
 //#include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -39,142 +41,262 @@ int		gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
+
+///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
+///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
+static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
+	//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+
+	const btScalar sum = btScalar(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_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+	return deltaImpulse;
 }
 
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+
+	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+	const btScalar sum = btScalar(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_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+	return deltaImpulse;
 }
 
+
+
 #ifdef USE_SIMD
 #include <emmintrin.h>
+
+
 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
 {
 	__m128 result = _mm_mul_ps( vec0, vec1);
 	return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
 }
-#endif//USE_SIMD
+
+#if defined (BT_ALLOW_SSE4)
+#include <intrin.h>
+
+#define USE_FMA					1
+#define USE_FMA3_INSTEAD_FMA4	1
+#define USE_SSE4_DOT			1
+
+#define SSE4_DP(a, b)			_mm_dp_ps(a, b, 0x7f)
+#define SSE4_DP_FP(a, b)		_mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
+
+#if USE_SSE4_DOT
+#define DOT_PRODUCT(a, b)		SSE4_DP(a, b)
+#else
+#define DOT_PRODUCT(a, b)		btSimdDot3(a, b)
+#endif
+
+#if USE_FMA
+#if USE_FMA3_INSTEAD_FMA4
+// a*b + c
+#define FMADD(a, b, c)		_mm_fmadd_ps(a, b, c)
+// -(a*b) + c
+#define FMNADD(a, b, c)		_mm_fnmadd_ps(a, b, c)
+#else // USE_FMA3
+// a*b + c
+#define FMADD(a, b, c)		_mm_macc_ps(a, b, c)
+// -(a*b) + c
+#define FMNADD(a, b, c)		_mm_nmacc_ps(a, b, c)
+#endif
+#else // USE_FMA
+// c + a*b
+#define FMADD(a, b, c)		_mm_add_ps(c, _mm_mul_ps(a, b))
+// c - a*b
+#define FMNADD(a, b, c)		_mm_sub_ps(c, _mm_mul_ps(a, b))
+#endif
+#endif
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& 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(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().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)));
-	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	btSimdScalar 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_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
+	btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().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)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	btSimdScalar 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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal2).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_add_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
+	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_add_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));
+	return deltaImpulse;
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+
+// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
+static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
 {
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+#if defined (BT_ALLOW_SSE4)
+	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
+	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
+	const __m128 upperLimit		= _mm_set_ps1(c.m_upperLimit);
+	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
+	const __m128 maskLower		= _mm_cmpgt_ps(tmp, lowerLimit);
+	const __m128 maskUpper		= _mm_cmpgt_ps(upperLimit, tmp);
+	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
+	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
+	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
+	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
+	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
+	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
+	return deltaImpulse;
+#else
+	return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
+#endif
+}
 
-//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
 
-	const btScalar sum = btScalar(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_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-}
 
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& 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(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().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)));
-	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	btSimdScalar 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_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
+	btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().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)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	btSimdScalar 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_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.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_add_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));
+	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_add_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));
+	return deltaImpulse;
+}
+
+
+// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
+static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+{
+#ifdef BT_ALLOW_SSE4
+	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
+	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
+	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
+	const __m128 mask			= _mm_cmpgt_ps(tmp, lowerLimit);
+	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
+	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, tmp, mask);
+	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
+	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
+	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
+	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
+	return deltaImpulse;
+#else
+	return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
+#endif //BT_ALLOW_SSE4
+}
+
+
+#endif //USE_SIMD
+
+
+
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
 #else
-	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+	return resolveSingleConstraintRowGeneric(body1,body2,c);
 #endif
 }
 
-// Projected Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+// Project Gauss Seidel or the equivalent Sequential Impulse
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
+}
 
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
-	const btScalar sum = btScalar(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_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
+#else
+	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
 }
 
 
@@ -243,6 +365,63 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 }
 
 
+ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
+	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
+	 m_btSeed2(0)
+ {
+
+#ifdef USE_SIMD
+	 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
+	 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
+#endif //USE_SIMD
+
+#ifdef BT_ALLOW_SSE4
+	 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
+	 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
+	 {
+		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+	 }
+#endif//BT_ALLOW_SSE4
+
+ }
+
+ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+ {
+ }
+
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_scalar_reference;
+ }
+
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
+ }
+
+
+#ifdef USE_SIMD
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_sse2;
+ }
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_sse2;
+ }
+#ifdef BT_ALLOW_SSE4
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
+ {
+	 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+ }
+ btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
+ {
+	 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+ }
+#endif //BT_ALLOW_SSE4
+#endif //USE_SIMD
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
@@ -303,7 +482,7 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 		solverBody->m_angularVelocity = rb->getAngularVelocity();
 		solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
 		solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
-		
+
 	} else
 	{
 		solverBody->m_worldTransform.setIdentity();
@@ -335,7 +514,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 
 void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
 {
-	
+
 
 	if (colObj && colObj->hasAnisotropicFriction(frictionMode))
 	{
@@ -356,7 +535,7 @@ void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 
-	
+
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
@@ -417,12 +596,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	}
 
 	{
-		
+
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
 			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
 			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
 		rel_vel = vel1Dotn+vel2Dotn;
@@ -431,12 +610,13 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 
         // Urho3D: possible friction fix from https://github.com/bulletphysics/bullet3/commit/907ac49892ede3f4a3295f7cbd96759107e5fc0e
 		btScalar velocityError =  desiredVelocity - rel_vel;
-		btScalar	velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
+		btScalar velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-		
+
 	}
 }
 
@@ -444,7 +624,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -452,7 +632,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 
 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity, btScalar cfmSlip)
 
 {
@@ -498,12 +678,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	}
 
 	{
-		
+
 
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
 			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
 			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
 		rel_vel = vel1Dotn+vel2Dotn;
@@ -512,12 +692,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 
         // Urho3D: possible friction fix from https://github.com/bulletphysics/bullet3/commit/907ac49892ede3f4a3295f7cbd96759107e5fc0e
 		btScalar velocityError =  desiredVelocity - rel_vel;
-		btScalar	velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
+		btScalar velocityImpulse = velocityError * btScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-		
+
 	}
 }
 
@@ -532,7 +712,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConst
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -560,7 +740,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			body.setCompanionId(solverBodyIdA);
 		} else
 		{
-			
+
 			if (m_fixedBodyId<0)
 			{
 				m_fixedBodyId = m_tmpSolverBodyPool.size();
@@ -578,15 +758,15 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 #include <stdio.h>
 
 
-void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
 																 int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 																 btScalar& relaxation,
 																 const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
-			
-			const btVector3& pos1 = cp.getPositionWorldOnA();
-			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+		//	const btVector3& pos1 = cp.getPositionWorldOnA();
+		//	const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 			btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -594,23 +774,23 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 			btRigidBody* rb0 = bodyA->m_originalBody;
 			btRigidBody* rb1 = bodyB->m_originalBody;
 
-//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 //			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
 			relaxation = 1.f;
 
 			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 
 				{
 #ifdef COMPUTE_IMPULSE_DENOM
 					btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 					btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else							
+#else
 					btVector3 vec;
 					btScalar denom0 = 0.f;
 					btScalar denom1 = 0.f;
@@ -624,7 +804,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 					}
-#endif //COMPUTE_IMPULSE_DENOM		
+#endif //COMPUTE_IMPULSE_DENOM
 
 					btScalar denom = relaxation/(denom0+denom1);
 					solverConstraint.m_jacDiagABInv = denom;
@@ -662,11 +842,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					btVector3 vel  = vel1 - vel2;
 					btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-					
+
 
 					solverConstraint.m_friction = cp.m_combinedFriction;
 
-				
+
 					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 					if (restitution <= btScalar(0.))
 					{
@@ -696,17 +876,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
 					btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
 					btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
-						
 
-					btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) 
+
+					btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
 						+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
-					btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) 
+					btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
 						+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
 					btScalar rel_vel = vel1Dotn+vel2Dotn;
 
 					btScalar positionalError = 0.f;
 					btScalar	velocityError = restitution - rel_vel;// * damping;
-					
+
 
 					btScalar erp = infoGlobal.m_erp2;
 					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -751,7 +931,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 
 
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
 																		int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
@@ -816,7 +996,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 	///avoid collision response between two static objects
-	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
 	int rollingFriction=1;
@@ -830,7 +1010,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos1;
 			btVector3 rel_pos2;
 			btScalar relaxation;
-			
+
 
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
@@ -844,7 +1024,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			const btVector3& pos1 = cp.getPositionWorldOnA();
 			const btVector3& pos2 = cp.getPositionWorldOnB();
 
-			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
 			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
@@ -852,13 +1032,13 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
 			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
-			
+
 			btVector3 vel  = vel1 - vel2;
 			btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
 			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
 
-			
+
 
 //			const btVector3& pos1 = cp.getPositionWorldOnA();
 //			const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -899,21 +1079,21 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					if (axis1.length()>0.001)
 						addRollingFrictionConstraint(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 
+			///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 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 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, 
+			///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
@@ -969,9 +1149,9 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			}
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-		
 
-			
+
+
 
 		}
 	}
@@ -1011,7 +1191,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 				bool found=false;
 				for (int b=0;b<numBodies;b++)
 				{
-                
+
 					if (&constraint->getRigidBodyA()==bodies[b])
 					{
 						found = true;
@@ -1043,7 +1223,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
             bool found=false;
             for (int b=0;b<numBodies;b++)
             {
-                
+
                 if (manifoldPtr[i]->getBody0()==bodies[b])
                 {
                     found = true;
@@ -1067,8 +1247,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
         }
     }
 #endif //BT_ADDITIONAL_DEBUG
-	
-	
+
+
 	for (int i = 0; i < numBodies; i++)
 	{
 		bodies[i]->setCompanionId(-1);
@@ -1083,6 +1263,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 	//convert all bodies
 
+
 	for (int i=0;i<numBodies;i++)
 	{
 		int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
@@ -1092,14 +1273,27 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 		{
 			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
 			btVector3 gyroForce (0,0,0);
-			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
-				gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
+				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
 				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
 			}
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+
+			}
+			
+
 		}
 	}
-	
+
 	if (1)
 	{
 		int j;
@@ -1119,7 +1313,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 			int totalNumRows = 0;
 			int i;
-			
+
 			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
 			//calculate the total number of contraint rows
 			for (i=0;i<numConstraints;i++)
@@ -1149,14 +1343,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			}
 			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 
-			
+
 			///setup the btSolverConstraints
 			int currentRow = 0;
 
 			for (i=0;i<numConstraints;i++)
 			{
 				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-				
+
 				if (info1.m_numConstraintRows)
 				{
 					btAssert(currentRow<totalNumRows);
@@ -1264,7 +1458,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						}
 
 
-						
+
 						{
 							btScalar rel_vel;
 							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
@@ -1272,11 +1466,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 							btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
 							btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
-							
-							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) 
+
+							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
 												+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
-							
-							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) 
+
+							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
 																+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
 
 							rel_vel = vel1Dotn+vel2Dotn;
@@ -1342,7 +1536,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-	
+
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
 		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
@@ -1355,7 +1549,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
-			//contact/friction constraints are not solved more than 
+			//contact/friction constraints are not solved more than
 			if (iteration< infoGlobal.m_numIterations)
 			{
 				for (int j=0; j<numConstraintPool; ++j) {
@@ -1434,7 +1628,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						{
 
 							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-				
+
 							if (totalImpulse>btScalar(0))
 							{
 								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
@@ -1456,12 +1650,11 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
-		
-				
+
+
 
 				///solve all friction constraints, using SIMD, if available
 
@@ -1476,12 +1669,11 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						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);
-						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 
-				
+
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
 				for (j=0;j<numRollingFrictionPoolConstraints;j++)
 				{
@@ -1500,9 +1692,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
 					}
 				}
-				
 
-			}			
+
+			}
 		}
 	} else
 	{
@@ -1626,10 +1818,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 
 		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
 		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
-		{			
+		{
 			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 		}
-		
+
 	}
 	return 0.f;
 }
@@ -1671,13 +1863,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 			fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
 			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
 			fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
-			
+
 		}
 
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
-
 		// Urho3D: if constraint has infinity breaking threshold, do not break no matter what
-		btScalar breakingThreshold = constr->getBreakingImpulseThreshold();
+        btScalar breakingThreshold = constr->getBreakingImpulseThreshold();
 		if (breakingThreshold < SIMD_INFINITY && btFabs(solverConstr.m_appliedImpulse)>=breakingThreshold)
 		{
 			constr->setEnabled(false);
@@ -1695,7 +1886,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 				m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
 			else
 				m_tmpSolverBodyPool[i].writebackVelocity();
-			
+
 			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
 				m_tmpSolverBodyPool[i].m_linearVelocity+
 				m_tmpSolverBodyPool[i].m_externalForceImpulse);
@@ -1728,13 +1919,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 
 	BT_PROFILE("solveGroup");
 	//you need to provide at least some bodies
-	
+
 	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
 	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
-	
+
 	return 0.f;
 }
 
@@ -1742,5 +1933,3 @@ void	btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
 }
-
-

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -27,6 +27,8 @@ class btCollisionObject;
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
+typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
@@ -43,6 +45,10 @@ protected:
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+
 	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
@@ -88,13 +94,10 @@ protected:
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
-	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 		
 protected:
 	
@@ -115,9 +118,7 @@ public:
 	virtual ~btSequentialImpulseConstraintSolver();
 
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-	
-
-	
+		
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
 	
@@ -139,6 +140,33 @@ public:
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;
 	}
+
+	btSingleConstraintRowSolver	getActiveConstraintRowSolverGeneric()
+	{
+		return m_resolveSingleConstraintRowGeneric;
+	}
+	void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver)
+	{
+		m_resolveSingleConstraintRowGeneric = rowSolver;
+	}
+	btSingleConstraintRowSolver	getActiveConstraintRowSolverLowerLimit()
+	{
+		return m_resolveSingleConstraintRowLowerLimit;
+	}
+	void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver)
+	{
+		m_resolveSingleConstraintRowLowerLimit = rowSolver;
+	}
+
+	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
+	btSingleConstraintRowSolver	getScalarConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverGeneric();
+
+	///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
+	btSingleConstraintRowSolver	getScalarConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverLowerLimit();
 };
 
 

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -539,8 +539,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			btScalar tag_vel = getTargetLinMotorVelocity();
 			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
 			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
-			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
-			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+			info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
+			info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
 		}
 		if(limit)
 		{
@@ -641,8 +641,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			}
 			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
 			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
-			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
-			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+			info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
+			info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
 		}
 		if(limit)
 		{

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -24,7 +24,7 @@ subject to the following restrictions:
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
 :btTypedObject(type),
 m_userConstraintType(-1),
-m_userConstraintId(-1),
+m_userConstraintPtr((void*)-1),
 m_breakingImpulseThreshold(SIMD_INFINITY),
 m_isEnabled(true),
 m_needsFeedback(false),
@@ -41,7 +41,7 @@ m_jointFeedback(0)
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
 :btTypedObject(type),
 m_userConstraintType(-1),
-m_userConstraintId(-1),
+m_userConstraintPtr((void*)-1),
 m_breakingImpulseThreshold(SIMD_INFINITY),
 m_isEnabled(true),
 m_needsFeedback(false),

Source/ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -44,6 +44,7 @@ enum btTypedConstraintType
 	D6_SPRING_CONSTRAINT_TYPE,
 	GEAR_CONSTRAINT_TYPE,
 	FIXED_CONSTRAINT_TYPE,
+	D6_SPRING_2_CONSTRAINT_TYPE,
 	MAX_CONSTRAINT_TYPE
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp

@@ -1,405 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
-	Work in progress, functionality will be added on demand.
-
-	If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
-*/
-
-#include "Bullet-C-Api.h"
-#include "btBulletDynamicsCommon.h"
-#include "LinearMath/btAlignedAllocator.h"
-
-
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"	
-#include "LinearMath/btMatrix3x3.h"
-#include "LinearMath/btTransform.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/CollisionShapes/btTriangleShape.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-
-
-/*
-	Create and Delete a Physics SDK	
-*/
-
-struct	btPhysicsSdk
-{
-
-//	btDispatcher*				m_dispatcher;
-//	btOverlappingPairCache*		m_pairCache;
-//	btConstraintSolver*			m_constraintSolver
-
-	btVector3	m_worldAabbMin;
-	btVector3	m_worldAabbMax;
-
-
-	//todo: version, hardware/optimization settings etc?
-	btPhysicsSdk()
-		:m_worldAabbMin(-1000,-1000,-1000),
-		m_worldAabbMax(1000,1000,1000)
-	{
-
-	}
-
-	
-};
-
-plPhysicsSdkHandle	plNewBulletSdk()
-{
-	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
-	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
-}
-
-void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
-{
-	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
-	btAlignedFree(phys);	
-}
-
-
-/* Dynamics World */
-plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
-{
-	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
-	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
-	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
-	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
-	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
-	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
-	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
-	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
-	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
-
-	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
-	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
-}
-void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
-{
-	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAlignedFree(dynamicsWorld);
-}
-
-void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	dynamicsWorld->stepSimulation(timeStep);
-}
-
-void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->addRigidBody(body);
-}
-
-void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->removeRigidBody(body);
-}
-
-/* Rigid Body  */
-
-plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
-{
-	btTransform trans;
-	trans.setIdentity();
-	btVector3 localInertia(0,0,0);
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	if (mass)
-	{
-		shape->calculateLocalInertia(mass,localInertia);
-	}
-	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
-	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
-	btRigidBody* body = new (mem)btRigidBody(rbci);
-	body->setWorldTransform(trans);
-	body->setUserPointer(user_data);
-	return (plRigidBodyHandle) body;
-}
-
-void plDeleteRigidBody(plRigidBodyHandle cbody)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
-	btAssert(body);
-	btAlignedFree( body);
-}
-
-
-/* Collision Shape definition */
-
-plCollisionShapeHandle plNewSphereShape(plReal radius)
-{
-	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
-	
-}
-	
-plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
-{
-	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
-	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
-}
-
-plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
-{
-	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
-	
-	const int numSpheres = 2;
-	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
-	btScalar radi[numSpheres] = {radius,radius};
-	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
-}
-plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
-	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
-}
-
-plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
-	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
-}
-
-/* Convex Meshes */
-plCollisionShapeHandle plNewConvexHullShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
-	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
-}
-
-
-/* Concave static triangle meshes */
-plMeshInterfaceHandle		   plNewMeshInterface()
-{
-	return 0;
-}
-
-plCollisionShapeHandle plNewCompoundShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
-	return (plCollisionShapeHandle) new (mem)btCompoundShape();
-}
-
-void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
-	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
-	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
-	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
-	btTransform	localTrans;
-	localTrans.setIdentity();
-	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
-	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
-	compoundShape->addChildShape(localTrans,childShape);
-}
-
-void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
-{
-	btQuaternion orn;
-	orn.setEuler(yaw,pitch,roll);
-	orient[0] = orn.getX();
-	orient[1] = orn.getY();
-	orient[2] = orn.getZ();
-	orient[3] = orn.getW();
-
-}
-
-
-//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
-//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
-
-
-void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
-	(void)colShape;
-	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
-	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
-	convexHullShape->addPoint(btVector3(x,y,z));
-
-}
-
-void plDeleteShape(plCollisionShapeHandle cshape)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btAlignedFree(shape);
-}
-void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
-	shape->setLocalScaling(scaling);	
-}
-
-
-
-void plSetPosition(plRigidBodyHandle object, const plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btVector3 pos(position[0],position[1],position[2]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setOrigin(pos);
-	body->setWorldTransform(worldTrans);
-}
-
-void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setRotation(orn);
-	body->setWorldTransform(worldTrans);
-}
-
-void	plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btTransform& worldTrans = body->getWorldTransform();
-	worldTrans.setFromOpenGLMatrix(matrix);
-}
-
-void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	body->getWorldTransform().getOpenGLMatrix(matrix);
-
-}
-
-void	plGetPosition(plRigidBodyHandle object,plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btVector3& pos = body->getWorldTransform().getOrigin();
-	position[0] = pos.getX();
-	position[1] = pos.getY();
-	position[2] = pos.getZ();
-}
-
-void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btQuaternion& orn = body->getWorldTransform().getRotation();
-	orientation[0] = orn.getX();
-	orientation[1] = orn.getY();
-	orientation[2] = orn.getZ();
-	orientation[3] = orn.getW();
-}
-
-
-
-//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
-{
-	btVector3 vp(p1[0], p1[1], p1[2]);
-	btTriangleShape trishapeA(vp, 
-				  btVector3(p2[0], p2[1], p2[2]), 
-				  btVector3(p3[0], p3[1], p3[2]));
-	trishapeA.setMargin(0.000001f);
-	btVector3 vq(q1[0], q1[1], q1[2]);
-	btTriangleShape trishapeB(vq, 
-				  btVector3(q2[0], q2[1], q2[2]), 
-				  btVector3(q3[0], q3[1], q3[2]));
-	trishapeB.setMargin(0.000001f);
-	
-	// btVoronoiSimplexSolver sGjkSimplexSolver;
-	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
-	
-	static btSimplexSolverInterface sGjkSimplexSolver;
-	sGjkSimplexSolver.reset();
-	
-	static btGjkEpaPenetrationDepthSolver Solver0;
-	static btMinkowskiPenetrationDepthSolver Solver1;
-		
-	btConvexPenetrationDepthSolver* Solver = NULL;
-	
-	Solver = &Solver1;	
-		
-	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
-	
-	convexConvex.m_catchDegeneracies = 1;
-	
-	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
-	
-	btPointCollector gjkOutput;
-	btGjkPairDetector::ClosestPointInput input;
-	
-		
-	btTransform tr;
-	tr.setIdentity();
-	
-	input.m_transformA = tr;
-	input.m_transformB = tr;
-	
-	convexConvex.getClosestPoints(input, gjkOutput, 0);
-	
-	
-	if (gjkOutput.m_hasResult)
-	{
-		
-		pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
-		pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
-		pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
-
-		pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
-		pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
-		pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
-		
-		normal[0] = gjkOutput.m_normalOnBInWorld[0];
-		normal[1] = gjkOutput.m_normalOnBInWorld[1];
-		normal[2] = gjkOutput.m_normalOnBInWorld[2];
-
-		return gjkOutput.m_distance;
-	}
-	return -1.0f;	
-}
-

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -35,6 +35,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
@@ -59,7 +60,7 @@ int firstHit=startHit;
 SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
-	
+
 	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
 	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
 	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
@@ -89,7 +90,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 	int						m_numConstraints;
 	btIDebugDraw*			m_debugDrawer;
 	btDispatcher*			m_dispatcher;
-	
+
 	btAlignedObjectArray<btCollisionObject*> m_bodies;
 	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
@@ -128,7 +129,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		m_constraints.resize (0);
 	}
 
-	
+
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
@@ -141,7 +142,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 			btTypedConstraint** startConstraint = 0;
 			int numCurConstraints = 0;
 			int i;
-			
+
 			//find the first constraint for this island
 			for (i=0;i<m_numConstraints;i++)
 			{
@@ -165,7 +166,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
 			} else
 			{
-				
+
 				for (i=0;i<numBodies;i++)
 					m_bodies.push_back(bodies[i]);
 				for (i=0;i<numManifolds;i++)
@@ -188,7 +189,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
 		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
 		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-			
+
 		m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
@@ -207,10 +208,10 @@ m_solverIslandCallback ( NULL ),
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(0),
+m_fixedTimeStep(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
 m_profileTimings(0),
-m_fixedTimeStep(0),
 m_latencyMotionStateInterpolation(true)
 
 {
@@ -318,6 +319,9 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 			}
 		}
 	}
+    if (getDebugDrawer())
+        getDebugDrawer()->flushLines();
+
 }
 
 void	btDiscreteDynamicsWorld::clearForces()
@@ -330,7 +334,7 @@ void	btDiscreteDynamicsWorld::clearForces()
 		//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
 		body->clearForces();
 	}
-}	
+}
 
 ///apply gravity, call this once per timestep
 void	btDiscreteDynamicsWorld::applyGravity()
@@ -461,18 +465,18 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
 #endif //BT_NO_PROFILE
-	
+
 	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-	
+
 	BT_PROFILE("internalSingleStepSimulation");
 
 	if(0 != m_internalPreTickCallback) {
 		(*m_internalPreTickCallback)(this, timeStep);
-	}	
+	}
 
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -485,20 +489,20 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 
     createPredictiveContacts(timeStep);
-    
+
 	///perform collision detection
 	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
-	
+
 	getSolverInfo().m_timeStep = timeStep;
-	
+
 
 
 	///solve contact and other joint constraints
 	solveConstraints(getSolverInfo());
-	
+
 	///CallbackTriggers();
 
 	///integrate transforms
@@ -507,12 +511,12 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 	///update vehicle simulation
 	updateActions(timeStep);
-	
+
 	updateActivationState( timeStep );
 
 	if(0 != m_internalTickCallback) {
 		(*m_internalTickCallback)(this, timeStep);
-	}	
+	}
 }
 
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -604,14 +608,14 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
 {
 	BT_PROFILE("updateActions");
-	
+
 	for ( int i=0;i<m_actions.size();i++)
 	{
 		m_actions[i]->updateAction( this, timeStep);
 	}
 }
-	
-	
+
+
 void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
@@ -632,7 +636,7 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
 						body->setActivationState( WANTS_DEACTIVATION );
-					if (body->getActivationState() == ISLAND_SLEEPING) 
+					if (body->getActivationState() == ISLAND_SLEEPING)
 					{
 						body->setAngularVelocity(btVector3(0,0,0));
 						body->setLinearVelocity(btVector3(0,0,0));
@@ -651,6 +655,9 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
 {
 	m_constraints.push_back(constraint);
+    //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
+    btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
+    
 	if (disableCollisionsBetweenLinkedBodies)
 	{
 		constraint->getRigidBodyA().addConstraintRef(constraint);
@@ -702,25 +709,25 @@ void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
-	
+
 	m_sortedConstraints.resize( m_constraints.size());
-	int i; 
+	int i;
 	for (i=0;i<getNumConstraints();i++)
 	{
 		m_sortedConstraints[i] = m_constraints[i];
 	}
 
 //	btAssert(0);
-		
-	
+
+
 
 	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
-	
+
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-	
+
 	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-	
+
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
 
@@ -741,10 +748,10 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
         for (int i=0;i<this->m_predictiveManifolds.size();i++)
         {
             btPersistentManifold* manifold = m_predictiveManifolds[i];
-            
+
             const btCollisionObject* colObj0 = manifold->getBody0();
             const btCollisionObject* colObj1 = manifold->getBody1();
-            
+
             if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
             {
@@ -752,7 +759,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
             }
         }
     }
-    
+
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
@@ -776,7 +783,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
-	
+
 }
 
 
@@ -792,7 +799,7 @@ public:
 	btDispatcher* m_dispatcher;
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 		m_me(me),
 		m_allowedPenetration(0.0f),
@@ -872,7 +879,7 @@ int gNumClampedCcdMotions=0;
 void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 {
 	BT_PROFILE("createPredictiveContacts");
-	
+
 	{
 		BT_PROFILE("release predictive contact manifolds");
 
@@ -894,7 +901,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
@@ -908,7 +915,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -938,14 +945,14 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-					
+
 						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
 						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
 
-						
+
 						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
 						m_predictiveManifolds.push_back(manifold);
-						
+
 						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
 						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
 
@@ -978,10 +985,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
-			
+
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -994,7 +1001,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -1024,7 +1031,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-						
+
 						//printf("clamped integration to hit fraction = %f\n",fraction);
 						body->setHitFraction(sweepResults.m_closestHitFraction);
 						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
@@ -1049,13 +1056,13 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 							printf("sm2=%f\n",sm2);
 						}
 #else
-						
+
 						//don't apply the collision response right now, it will happen next frame
 						//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
 						//btScalar appliedImpulse = 0.f;
 						//btScalar depth = 0.f;
 						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
-						
+
 
 #endif
 
@@ -1063,10 +1070,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					}
 				}
 			}
-			
+
 
 			body->proceedToTransform( predictedTrans);
-		
+
 		}
 
 	}
@@ -1080,7 +1087,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			btPersistentManifold* manifold = m_predictiveManifolds[i];
 			btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
 			btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
-			
+
 			for (int p=0;p<manifold->getNumContacts();p++)
 			{
 				const btManifoldPoint& pt = manifold->getContactPoint(p);
@@ -1090,11 +1097,11 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
 				{
 					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
-				
+
 					const btVector3& pos1 = pt.getPositionWorldOnA();
 					const btVector3& pos2 = pt.getPositionWorldOnB();
 
-					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); 
+					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
 					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
 
 					if (body0)
@@ -1105,7 +1112,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-	
+
 }
 
 
@@ -1144,7 +1151,7 @@ void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 
 
 
-	
+
 
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
@@ -1164,12 +1171,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				btTransform tr;
 				tr.setIdentity();
 				btVector3 pivot = p2pC->getPivotInA();
-				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				// that ideally should draw the same frame	
+				// that ideally should draw the same frame
 				pivot = p2pC->getPivotInB();
-				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 			}
@@ -1188,13 +1195,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					break;
 				}
 				bool drawSect = true;
-				if(minAng > maxAng)
+				if(!pHinge->hasLimit())
 				{
 					minAng = btScalar(0.f);
 					maxAng = SIMD_2_PI;
 					drawSect = false;
 				}
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					btVector3& center = tr.getOrigin();
 					btVector3 normal = tr.getBasis().getColumn(2);
@@ -1229,7 +1236,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
 
 						pPrev = pCur;
-					}						
+					}
 					btScalar tws = pCT->getTwistSpan();
 					btScalar twa = pCT->getTwistAngle();
 					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
@@ -1257,7 +1264,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 				tr = p6DOF->getCalculatedTransformB();
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					tr = p6DOF->getCalculatedTransformA();
 					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
@@ -1298,6 +1305,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
+		///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
+		case D6_SPRING_2_CONSTRAINT_TYPE:
+		{
+			{
+				btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
+				btTransform tr = p6DOF->getCalculatedTransformA();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformB();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if (drawLimits)
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if (minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
+					}
+					else if (minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
+				}
+			}
+			break;
+		}
 		case SLIDER_CONSTRAINT_TYPE:
 			{
 				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
@@ -1320,7 +1378,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
-		default : 
+		default :
 			break;
 	}
 	return;
@@ -1420,19 +1478,19 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 		worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
 		worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
 		worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
-		
+
 		worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
 		worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
 		worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
 		worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
-		
+
 		worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
 		worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
 		worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
 		worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
-		
+
 		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
-	
+
 #ifdef BT_USE_DOUBLE_PRECISION
 		const char* structType = "btDynamicsWorldDoubleData";
 #else//BT_USE_DOUBLE_PRECISION
@@ -1448,10 +1506,10 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeDynamicsWorldInfo(serializer);
 
-	serializeRigidBodies(serializer);
-
 	serializeCollisionObjects(serializer);
 
+	serializeRigidBodies(serializer);
+
 	serializer->finishSerialization();
 }
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -151,7 +151,7 @@ public:
 	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 
 
-	void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
 
 	virtual void	debugDrawWorld();
 

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -87,7 +87,7 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
 	updateInertiaTensor();
 
-	m_rigidbodyFlags = 0;
+	m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
 
 
 	m_deltaLinearVelocity.setZero();
@@ -257,12 +257,41 @@ void btRigidBody::updateInertiaTensor()
 }
 
 
-btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
+
+btVector3 btRigidBody::getLocalInertia() const
 {
+
 	btVector3 inertiaLocal;
-	inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
-	inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
-	inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
+	const btVector3 inertia = m_invInertiaLocal;
+	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
+		inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+		inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+	return inertiaLocal;
+}
+
+inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+	const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
+	return w2;
+}
+
+inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+
+	btMatrix3x3 w1x, Iw1x;
+	const btVector3 Iwi = (I*w1);
+	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
+	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
+
+	const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
+	return dfw1;
+}
+
+btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
+{
+	btVector3 inertiaLocal = getLocalInertia();
 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
 	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
 	btVector3 gf = getAngularVelocity().cross(tmp);
@@ -274,6 +303,92 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
 	return gf;
 }
 
+void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a)
+{
+	const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2];
+	res.setValue(0, +a_2, -a_1,
+		-a_2, 0, +a_0,
+		+a_1, -a_0, 0);
+}
+
+btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
+{	
+	btVector3 idl = getLocalInertia();
+	btVector3 omega1 = getAngularVelocity();
+	btQuaternion q = getWorldTransform().getRotation();
+	
+	// Convert to body coordinates
+	btVector3 omegab = quatRotate(q.inverse(), omega1);
+	btMatrix3x3 Ib;
+	Ib.setValue(idl.x(),0,0,
+				0,idl.y(),0,
+				0,0,idl.z());
+	
+	btVector3 ibo = Ib*omegab;
+
+	// Residual vector
+	btVector3 f = step * omegab.cross(ibo);
+	
+	btMatrix3x3 skew0;
+	omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
+	btVector3 om = Ib*omegab;
+	btMatrix3x3 skew1;
+	om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
+	
+	// Jacobian
+	btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
+	
+//	btMatrix3x3 Jinv = J.inverse();
+//	btVector3 omega_div = Jinv*f;
+	btVector3 omega_div = J.solve33(f);
+	
+	// Single Newton-Raphson update
+	omegab = omegab - omega_div;//Solve33(J, f);
+	// Back to world coordinates
+	btVector3 omega2 = quatRotate(q,omegab);
+	btVector3 gf = omega2-omega1;
+	return gf;
+}
+
+
+
+btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
+{
+	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
+	// calculate using implicit euler step so it's stable.
+
+	const btVector3 inertiaLocal = getLocalInertia();
+	const btVector3 w0 = getAngularVelocity();
+
+	btMatrix3x3 I;
+
+	I = m_worldTransform.getBasis().scaled(inertiaLocal) *
+		m_worldTransform.getBasis().transpose();
+
+	// use newtons method to find implicit solution for new angular velocity (w')
+	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
+	// df/dw' = I + 1xIw'*step + w'xI*step
+
+	btVector3 w1 = w0;
+
+	// one step of newton's method
+	{
+		const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
+		const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
+
+		btVector3 dw;
+		dw = dfw.solve33(fw);
+		//const btMatrix3x3 dfw_inv = dfw.inverse();
+		//dw = dfw_inv*fw;
+
+		w1 -= dw;
+	}
+
+	btVector3 gf = (w1 - w0);
+	return gf;
+}
+
+
 void btRigidBody::integrateVelocities(btScalar step) 
 {
 	if (isStaticOrKinematicObject())
@@ -317,38 +432,50 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 }
 
 
-bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
-{
-	const btRigidBody* otherRb = btRigidBody::upcast(co);
-	if (!otherRb)
-		return true;
-
-	for (int i = 0; i < m_constraintRefs.size(); ++i)
-	{
-		const btTypedConstraint* c = m_constraintRefs[i];
-		if (c->isEnabled())
-			if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
-				return false;
-	}
-
-	return true;
-}
 
 
 
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
+	///disable collision with the 'other' body
+
 	int index = m_constraintRefs.findLinearSearch(c);
+	//don't add constraints that are already referenced
+	//btAssert(index == m_constraintRefs.size());
 	if (index == m_constraintRefs.size())
-		m_constraintRefs.push_back(c); 
-
-	m_checkCollideWith = true;
+	{
+		m_constraintRefs.push_back(c);
+		btCollisionObject* colObjA = &c->getRigidBodyA();
+		btCollisionObject* colObjB = &c->getRigidBodyB();
+		if (colObjA == this)
+		{
+			colObjA->setIgnoreCollisionCheck(colObjB, true);
+		}
+		else
+		{
+			colObjB->setIgnoreCollisionCheck(colObjA, true);
+		}
+	} 
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
-	m_constraintRefs.remove(c);
-	m_checkCollideWith = m_constraintRefs.size() > 0;
+	int index = m_constraintRefs.findLinearSearch(c);
+	//don't remove constraints that are not referenced
+	if(index < m_constraintRefs.size())
+    {
+        m_constraintRefs.remove(c);
+        btCollisionObject* colObjA = &c->getRigidBodyA();
+        btCollisionObject* colObjB = &c->getRigidBodyB();
+        if (colObjA == this)
+        {
+            colObjA->setIgnoreCollisionCheck(colObjB, false);
+        }
+        else
+        {
+            colObjB->setIgnoreCollisionCheck(colObjA, false);
+        }
+    }
 }
 
 int	btRigidBody::calculateSerializeBufferSize()	const

Source/ThirdParty/Bullet/src/BulletDynamics/Dynamics/btRigidBody.h

@@ -41,10 +41,13 @@ extern bool gDisableDeactivation;
 enum	btRigidBodyFlags
 {
 	BT_DISABLE_WORLD_GRAVITY = 1,
-	///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
-	///So generally it is best to not enable it. 
-	///If really needed, run at a high frequency like 1000 Hertz:	///See Demos/GyroscopicDemo for an example use
-	BT_ENABLE_GYROPSCOPIC_FORCE = 2
+	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
+	///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
+	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
+	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
+	BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
 };
 
 
@@ -87,7 +90,7 @@ class btRigidBody  : public btCollisionObject
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
 
-	//keep track of typed constraints referencing this rigid body
+	//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
 	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
 
 	int				m_rigidbodyFlags;
@@ -506,8 +509,6 @@ public:
 		return (getBroadphaseProxy() != 0);
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
-
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 
@@ -531,7 +532,18 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-	btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
+
+	
+
+	///perform implicit force computation in world space
+	btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
+	
+	///perform implicit force computation in body space (inertial frame)
+	btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
+
+	///explicit version is best avoided, it gains energy
+	btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
+	btVector3 getLocalInertia() const;
 
 	///////////////////////////////////////////////
 

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBody.h

@@ -30,6 +30,17 @@
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define btMultiBodyData	btMultiBodyDoubleData
+	#define btMultiBodyDataName	"btMultiBodyDoubleData"
+	#define btMultiBodyLinkData btMultiBodyLinkDoubleData
+	#define btMultiBodyLinkDataName	"btMultiBodyLinkDoubleData"
+#else
+	#define btMultiBodyData	btMultiBodyFloatData
+	#define btMultiBodyDataName	"btMultiBodyFloatData"
+	#define btMultiBodyLinkData btMultiBodyLinkFloatData
+	#define btMultiBodyLinkDataName	"btMultiBodyLinkFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
@@ -45,42 +56,73 @@ public:
     // initialization
     //
     
-    btMultiBody(int n_links,                // NOT including the base
-              btScalar mass,                // mass of base
-              const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
-              bool fixed_base_,           // whether the base is fixed (true) or can move (false)
-              bool can_sleep_);
+	btMultiBody(int n_links,             // NOT including the base
+		btScalar mass,                // mass of base
+		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
+		bool fixedBase,           // whether the base is fixed (true) or can move (false)
+		bool canSleep,
+		bool multiDof = false
+			  );
 
-    ~btMultiBody();
+
+	virtual ~btMultiBody();
     
-    void setupPrismatic(int i,             // 0 to num_links-1
-                        btScalar mass,
-                        const btVector3 &inertia,       // in my frame; assumed diagonal
-                        int parent,
-                        const btQuaternion &rot_parent_to_this,  // rotate points in parent frame to my frame.
-                        const btVector3 &joint_axis,             // in my frame
-                        const btVector3 &r_vector_when_q_zero,  // vector from parent COM to my COM, in my frame, when q = 0.
-						bool disableParentCollision=false
-						);
-
-    void setupRevolute(int i,            // 0 to num_links-1
+	void setupFixed(int linkIndex,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
+						   bool disableParentCollision);
+
+						
+	void setupPrismatic(int i,
+                               btScalar mass,
+                               const btVector3 &inertia,
+                               int parent,
+                               const btQuaternion &rotParentToThis,
+                               const btVector3 &jointAxis,
+                               const btVector3 &parentComToThisPivotOffset,
+							   const btVector3 &thisPivotToThisComOffset,
+							   bool disableParentCollision);
+
+    void setupRevolute(int linkIndex,            // 0 to num_links-1
                        btScalar mass,
                        const btVector3 &inertia,
-                       int parent,
-                       const btQuaternion &zero_rot_parent_to_this,  // rotate points in parent frame to this frame, when q = 0
-                       const btVector3 &joint_axis,    // in my frame
-                       const btVector3 &parent_axis_position,    // vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &my_axis_position,       // vector from joint axis to my COM, in MY frame
+                       int parentIndex,
+                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+                       const btVector3 &jointAxis,    // in my frame
+                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
 					   bool disableParentCollision=false);
+
+	void setupSpherical(int linkIndex,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision=false);		
+
+	void setupPlanar(int i,											// 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
+					   const btVector3 &rotationAxis,
+                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame                       
+					   bool disableParentCollision=false);		
 	
 	const btMultibodyLink& getLink(int index) const
 	{
-		return links[index];
+		return m_links[index];
 	}
 
 	btMultibodyLink& getLink(int index)
 	{
-		return links[index];
+		return m_links[index];
 	}
 
 
@@ -106,69 +148,98 @@ public:
     
     
     //
-    // get number of links, masses, moments of inertia
+    // get number of m_links, masses, moments of inertia
     //
 
-    int getNumLinks() const { return links.size(); }
-    btScalar getBaseMass() const { return base_mass; }
-    const btVector3 & getBaseInertia() const { return base_inertia; }
+    int getNumLinks() const { return m_links.size(); }
+	int getNumDofs() const { return m_dofCount; }
+	int getNumPosVars() const { return m_posVarCnt; }
+    btScalar getBaseMass() const { return m_baseMass; }
+    const btVector3 & getBaseInertia() const { return m_baseInertia; }
     btScalar getLinkMass(int i) const;
     const btVector3 & getLinkInertia(int i) const;
     
+	
 
     //
     // change mass (incomplete: can only change base mass and inertia at present)
     //
 
-    void setBaseMass(btScalar mass) { base_mass = mass; }
-    void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
+    void setBaseMass(btScalar mass) { m_baseMass = mass; }
+    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
 
 
     //
     // get/set pos/vel/rot/omega for the base link
     //
 
-    const btVector3 & getBasePos() const { return base_pos; }    // in world frame
+    const btVector3 & getBasePos() const { return m_basePos; }    // in world frame
     const btVector3 getBaseVel() const 
 	{ 
-		return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); 
+		return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 
 	}     // in world frame
     const btQuaternion & getWorldToBaseRot() const 
 	{ 
-		return base_quat; 
+		return m_baseQuat; 
 	}     // rotates world vectors into base frame
-    btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); }   // in world frame
+    btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); }   // in world frame
 
     void setBasePos(const btVector3 &pos) 
 	{ 
-		base_pos = pos; 
+		m_basePos = pos; 
+	}
+
+	void setBaseWorldTransform(const btTransform& tr)
+	{
+		setBasePos(tr.getOrigin());
+		setWorldToBaseRot(tr.getRotation().inverse());
+
+	}
+
+	btTransform getBaseWorldTransform() const
+	{
+		btTransform tr;
+		tr.setOrigin(getBasePos());
+		tr.setRotation(getWorldToBaseRot().inverse());
+		return tr;
 	}
+
     void setBaseVel(const btVector3 &vel) 
 	{ 
 
-		m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; 
+		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 
 	}
     void setWorldToBaseRot(const btQuaternion &rot) 
 	{ 
-		base_quat = rot; 
+		m_baseQuat = rot;					//m_baseQuat asumed to ba alias!?
 	}
     void setBaseOmega(const btVector3 &omega) 
 	{ 
-		m_real_buf[0]=omega[0]; 
-		m_real_buf[1]=omega[1]; 
-		m_real_buf[2]=omega[2]; 
+		m_realBuf[0]=omega[0]; 
+		m_realBuf[1]=omega[1]; 
+		m_realBuf[2]=omega[2]; 
 	}
 
 
     //
-    // get/set pos/vel for child links (i = 0 to num_links-1)
+    // get/set pos/vel for child m_links (i = 0 to num_links-1)
     //
 
     btScalar getJointPos(int i) const;
     btScalar getJointVel(int i) const;
 
+	btScalar * getJointVelMultiDof(int i);
+	btScalar * getJointPosMultiDof(int i);
+
+	const btScalar * getJointVelMultiDof(int i) const ;
+	const btScalar * getJointPosMultiDof(int i) const ;
+
     void setJointPos(int i, btScalar q);
     void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, btScalar *q);
+    void setJointVelMultiDof(int i, btScalar *qdot);	
+
+
 
     //
     // direct access to velocities as a vector of 6 + num_links elements.
@@ -176,7 +247,7 @@ public:
     //
     const btScalar * getVelocityVector() const 
 	{ 
-		return &m_real_buf[0]; 
+		return &m_realBuf[0]; 
 	}
 /*    btScalar * getVelocityVector() 
 	{ 
@@ -185,7 +256,7 @@ public:
   */  
 
     //
-    // get the frames of reference (positions and orientations) of the child links
+    // get the frames of reference (positions and orientations) of the child m_links
     // (i = 0 to num_links-1)
     //
 
@@ -216,22 +287,37 @@ public:
     //
 
     void clearForcesAndTorques();
+   void clearConstraintForces();
+
 	void clearVelocities();
 
     void addBaseForce(const btVector3 &f) 
 	{ 
-		base_force += f; 
+		m_baseForce += f; 
 	}
-    void addBaseTorque(const btVector3 &t) { base_torque += t; }
+    void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
     void addLinkForce(int i, const btVector3 &f);
     void addLinkTorque(int i, const btVector3 &t);
-    void addJointTorque(int i, btScalar Q);
 
-    const btVector3 & getBaseForce() const { return base_force; }
-    const btVector3 & getBaseTorque() const { return base_torque; }
+ void addBaseConstraintForce(const btVector3 &f)
+        {
+                m_baseConstraintForce += f;
+        }
+    void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
+    void addLinkConstraintForce(int i, const btVector3 &f);
+    void addLinkConstraintTorque(int i, const btVector3 &t);
+       
+
+void addJointTorque(int i, btScalar Q);
+	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
+	void addJointTorqueMultiDof(int i, const btScalar *Q);
+
+    const btVector3 & getBaseForce() const { return m_baseForce; }
+    const btVector3 & getBaseTorque() const { return m_baseTorque; }
     const btVector3 & getLinkForce(int i) const;
     const btVector3 & getLinkTorque(int i) const;
     btScalar getJointTorque(int i) const;
+	btScalar * getJointTorqueMultiDof(int i);
 
 
     //
@@ -255,6 +341,13 @@ public:
                         btAlignedObjectArray<btVector3> &scratch_v,
                         btAlignedObjectArray<btMatrix3x3> &scratch_m);
 
+	void stepVelocitiesMultiDof(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
+			bool isConstraintPass=false
+		);
+
     // calcAccelerationDeltas
     // input: force vector (in same format as jacobian, i.e.:
     //                      3 torque values, 3 force values, num_links joint torque values)
@@ -265,13 +358,17 @@ public:
                                 btAlignedObjectArray<btScalar> &scratch_r,
                                 btAlignedObjectArray<btVector3> &scratch_v) const;
 
+	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+                                btAlignedObjectArray<btScalar> &scratch_r,
+                                btAlignedObjectArray<btVector3> &scratch_v) const;
+
     // apply a delta-vee directly. used in sequential impulses code.
     void applyDeltaVee(const btScalar * delta_vee) 
 	{
 
         for (int i = 0; i < 6 + getNumLinks(); ++i) 
 		{
-			m_real_buf[i] += delta_vee[i];
+			m_realBuf[i] += delta_vee[i];
 		}
 		
     }
@@ -300,12 +397,58 @@ public:
         for (int i = 0; i < 6 + getNumLinks(); ++i)
 		{
 			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
-			m_real_buf[i] += delta_vee[i] * multiplier;
+			m_realBuf[i] += delta_vee[i] * multiplier;
+			btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
+		}
+    }
+
+	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
+	{
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+                {
+                        m_deltaV[dof] += delta_vee[dof] * multiplier;
+                }
+	}
+	void processDeltaVeeMultiDof2()
+	{
+		applyDeltaVeeMultiDof(&m_deltaV[0],1);
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+                {
+			m_deltaV[dof] = 0.f;
+		}
+	}
+
+	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
+	{
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//	printf("%.4f ", delta_vee[dof]*multiplier);
+		//printf("\n");
+
+		//btScalar sum = 0;
+		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		//{
+		//	sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
+		//}
+		//btScalar l = btSqrt(sum);
+
+		//if (l>m_maxAppliedImpulse)
+		//{
+		//	multiplier *= m_maxAppliedImpulse/l;
+		//}
+
+		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+		{
+			m_realBuf[dof] += delta_vee[dof] * multiplier;
+			btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
 		}
     }
 
+	
+	
     // timestep the positions (given current velocities).
     void stepPositions(btScalar dt);
+	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
 
 
     //
@@ -323,23 +466,48 @@ public:
                              btAlignedObjectArray<btVector3> &scratch_v,
                              btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
 
+	//multidof version of fillContactJacobian
+	void fillContactJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+                             const btVector3 &normal,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+
+	//a more general version of fillContactJacobianMultiDof which does not assume..
+	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
+	void filConstraintJacobianMultiDof(int link,
+                             const btVector3 &contact_point,
+							 const btVector3 &normal_ang,
+                             const btVector3 &normal_lin,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
 
     //
     // sleeping
     //
 	void	setCanSleep(bool canSleep)
 	{
-		can_sleep = canSleep;
+		m_canSleep = canSleep;
 	}
 
-    bool isAwake() const { return awake; }
+	bool getCanSleep()const
+	{
+		return m_canSleep;
+	}
+
+    bool isAwake() const { return m_awake; }
     void wakeUp();
     void goToSleep();
     void checkMotionAndSleepIfRequired(btScalar timestep);
     
 	bool hasFixedBase() const
 	{
-		    return fixed_base;
+		    return m_fixedBase;
 	}
 
 	int getCompanionId() const
@@ -352,9 +520,9 @@ public:
 		m_companionId = id;
 	}
 
-	void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links
+	void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
 	{
-		links.resize(numLinks);
+		m_links.resize(numLinks);
 	}
 
 	btScalar getLinearDamping() const
@@ -369,6 +537,10 @@ public:
 	{
 		return m_angularDamping;
 	}
+	void setAngularDamping( btScalar damp)
+	{
+		m_angularDamping = damp;
+	}
 		
 	bool getUseGyroTerm() const
 	{
@@ -378,6 +550,15 @@ public:
 	{
 		m_useGyroTerm = useGyro;
 	}
+	btScalar	getMaxCoordinateVelocity() const
+	{
+		return m_maxCoordinateVelocity ;
+	}
+	void	setMaxCoordinateVelocity(btScalar maxVel)
+	{
+		m_maxCoordinateVelocity = maxVel;
+	}
+
 	btScalar	getMaxAppliedImpulse() const
 	{
 		return m_maxAppliedImpulse;
@@ -386,7 +567,6 @@ public:
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
-
 	void	setHasSelfCollision(bool hasSelfCollision)
 	{
 		m_hasSelfCollision = hasSelfCollision;
@@ -396,6 +576,47 @@ public:
 		return m_hasSelfCollision;
 	}
 
+	bool isMultiDof() { return m_isMultiDof; }
+	void finalizeMultiDof();
+
+	void useRK4Integration(bool use) { m_useRK4 = use; }
+	bool isUsingRK4Integration() const { return m_useRK4; }
+	void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
+	bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
+
+	bool isPosUpdated() const
+	{
+		return __posUpdated;
+	}
+	void setPosUpdated(bool updated)
+	{
+		__posUpdated = updated;
+	}
+	
+	//internalNeedsJointFeedback is for internal use only
+	bool internalNeedsJointFeedback() const
+	{
+		return m_internalNeedsJointFeedback;
+	}
+	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+
+	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+	
+	virtual	int	calculateSerializeBufferSize()	const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+
+	const char*				getBaseName() const
+	{
+		return m_baseName;
+	}
+	///memory of setBaseName needs to be manager by user
+	void	setBaseName(const char* name)
+	{
+		m_baseName = name;
+	}
+
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -403,64 +624,181 @@ private:
     void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
 
 	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
-    
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+	
+	void updateLinksDofOffsets()
+	{
+		int dofOffset = 0, cfgOffset = 0;
+		for(int bidx = 0; bidx < m_links.size(); ++bidx)
+		{
+			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
+			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
+		}
+	}
+
+	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+	
 	
 private:
 
 	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
+	const char*				m_baseName;//memory needs to be manager by user!
 
-    btVector3 base_pos;       // position of COM of base (world frame)
-    btQuaternion base_quat;   // rotates world points into base frame
+    btVector3 m_basePos;       // position of COM of base (world frame)
+    btQuaternion m_baseQuat;   // rotates world points into base frame
 
-    btScalar base_mass;         // mass of the base
-    btVector3 base_inertia;   // inertia of the base (in local frame; diagonal)
+    btScalar m_baseMass;         // mass of the base
+    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)
 
-    btVector3 base_force;     // external force applied to base. World frame.
-    btVector3 base_torque;    // external torque applied to base. World frame.
-    
-    btAlignedObjectArray<btMultibodyLink> links;    // array of links, excluding the base. index from 0 to num_links-1.
+    btVector3 m_baseForce;     // external force applied to base. World frame.
+    btVector3 m_baseTorque;    // external torque applied to base. World frame.
+   
+    btVector3 m_baseConstraintForce;     // external force applied to base. World frame.
+    btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.
+ 
+    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
 	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
+
     
     //
-    // real_buf:
+    // realBuf:
     //  offset         size            array
-    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)
+    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
     //   6+num_links    num_links       D
     //
-    // vector_buf:
+    // vectorBuf:
     //  offset         size         array
     //   0              num_links    h_top
     //   num_links      num_links    h_bottom
     //
-    // matrix_buf:
+    // matrixBuf:
     //  offset         size         array
     //   0              num_links+1  rot_from_parent
     //
-    
-    btAlignedObjectArray<btScalar> m_real_buf;
-    btAlignedObjectArray<btVector3> vector_buf;
-    btAlignedObjectArray<btMatrix3x3> matrix_buf;
+   btAlignedObjectArray<btScalar> m_deltaV; 
+    btAlignedObjectArray<btScalar> m_realBuf;
+    btAlignedObjectArray<btVector3> m_vectorBuf;
+    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 
-    //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
 
-	btMatrix3x3 cached_inertia_top_left;
-	btMatrix3x3 cached_inertia_top_right;
-	btMatrix3x3 cached_inertia_lower_left;
-	btMatrix3x3 cached_inertia_lower_right;
+	btMatrix3x3 m_cachedInertiaTopLeft;
+	btMatrix3x3 m_cachedInertiaTopRight;
+	btMatrix3x3 m_cachedInertiaLowerLeft;
+	btMatrix3x3 m_cachedInertiaLowerRight;
 
-    bool fixed_base;
+    bool m_fixedBase;
 
     // Sleep parameters.
-    bool awake;
-    bool can_sleep;
-    btScalar sleep_timer;
+    bool m_awake;
+    bool m_canSleep;
+    btScalar m_sleepTimer;
 
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
 	bool	m_useGyroTerm;
 	btScalar	m_maxAppliedImpulse;
+	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
+	bool		m_isMultiDof;
+		bool __posUpdated;
+		int m_dofCount, m_posVarCnt;
+	bool m_useRK4, m_useGlobalVelocities;
+	
+	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
+	bool m_internalNeedsJointFeedback;
 };
 
+struct btMultiBodyLinkDoubleData
+{
+	btQuaternionDoubleData	m_zeroRotParentToThis;
+	btVector3DoubleData		m_parentComToThisComOffset;
+	btVector3DoubleData		m_thisPivotToThisComOffset;
+	btVector3DoubleData		m_jointAxisTop[6];
+	btVector3DoubleData		m_jointAxisBottom[6];
+
+
+	char					*m_linkName;
+	char					*m_jointName;
+	btCollisionObjectDoubleData	*m_linkCollider;
+	
+	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	double					m_linkMass;
+	int						m_parentIndex;
+	int						m_jointType;
+	
+
+	
+
+	int						m_dofCount;
+	int						m_posVarCount;
+	double					m_jointPos[7];
+	double					m_jointVel[6];
+	double					m_jointTorque[6];
+	
+	
+	
+};
+
+struct btMultiBodyLinkFloatData
+{
+	btQuaternionFloatData	m_zeroRotParentToThis;
+	btVector3FloatData		m_parentComToThisComOffset;
+	btVector3FloatData		m_thisPivotToThisComOffset;
+	btVector3FloatData		m_jointAxisTop[6];
+	btVector3FloatData		m_jointAxisBottom[6];
+	
+
+	char				*m_linkName;
+	char				*m_jointName;
+	btCollisionObjectFloatData	*m_linkCollider;
+	
+	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
+	int						m_dofCount;
+	float				m_linkMass;
+	int					m_parentIndex;
+	int					m_jointType;
+	
+
+		
+	float					m_jointPos[7];
+	float					m_jointVel[6];
+	float					m_jointTorque[6];
+	int						m_posVarCount;
+	
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btMultiBodyDoubleData
+{
+	char	*m_baseName;
+	btMultiBodyLinkDoubleData	*m_links;
+	btCollisionObjectDoubleData	*m_baseCollider;
+
+	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	
+	int		m_numLinks;
+	double	m_baseMass;
+
+	char m_padding[4];
+	
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btMultiBodyFloatData
+{
+	char	*m_baseName;
+	btMultiBodyLinkFloatData	*m_links;
+	btCollisionObjectFloatData	*m_baseCollider;
+	btTransformFloatData m_baseWorldTransform;
+	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	
+	float	m_baseMass;
+	int		m_numLinks;
+};
+
+
+
 #endif

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -1,263 +1,107 @@
 #include "btMultiBodyConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+
+
 
 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
 	:m_bodyA(bodyA),
 	m_bodyB(bodyB),
 	m_linkA(linkA),
 	m_linkB(linkB),
-	m_num_rows(numRows),
+	m_numRows(numRows),
+	m_jacSizeA(0),
+	m_jacSizeBoth(0),
 	m_isUnilateral(isUnilateral),
+	m_numDofsFinalized(-1),
 	m_maxAppliedImpulse(100)
 {
-	m_jac_size_A = (6 + bodyA->getNumLinks());
-	m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
-	m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
-	m_data.resize((2 + m_jac_size_both) * m_num_rows);
-}
 
-btMultiBodyConstraint::~btMultiBodyConstraint()
-{
 }
 
-
-
-btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
-														btMultiBodyJacobianData& data,
-														btScalar* jacOrgA,btScalar* jacOrgB,
-														const btContactSolverInfo& infoGlobal,
-														btScalar desiredVelocity,
-														btScalar lowerLimit,
-														btScalar upperLimit)
+void btMultiBodyConstraint::updateJacobianSizes()
 {
-			
-	
-	
-	constraintRow.m_multiBodyA = m_bodyA;
-	constraintRow.m_multiBodyB = m_bodyB;
-
-	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
-	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
-
-	if (multiBodyA)
+    if(m_bodyA)
 	{
-		
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
-
-		if (constraintRow.m_deltaVelAindex <0)
-		{
-			constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
-			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
-		}
-
-		constraintRow.m_jacAindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		for (int i=0;i<ndofA;i++)
-			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
-		
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	} 
-
-	if (multiBodyB)
-	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (constraintRow.m_deltaVelBindex <0)
-		{
-			constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
-			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
-		}
-
-		constraintRow.m_jacBindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-
-		for (int i=0;i<ndofB;i++)
-			data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
-
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
-	} 
-	{
-						
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			jacA = &data.m_jacobians[constraintRow.m_jacAindex];
-			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
-				denom0 += j*l;
-			}
-		} 
-		if (multiBodyB)
-		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
-			jacB = &data.m_jacobians[constraintRow.m_jacBindex];
-			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
-				denom1 += j*l;
-			}
-
-		} 
-
-		 if (multiBodyA && (multiBodyA==multiBodyB))
-		 {
-            // ndof1 == ndof2 in this case
-            for (int i = 0; i < ndofA; ++i) 
-			{
-                denom1 += jacB[i] * lambdaA[i];
-                denom1 += jacA[i] * lambdaB[i];
-            }
-        }
-
-		 btScalar d = denom0+denom1;
-		 if (btFabs(d)>SIMD_EPSILON)
-		 {
-			 
-			 constraintRow.m_jacDiagABInv = 1.f/(d);
-		 } else
-		 {
-			constraintRow.m_jacDiagABInv  = 1.f;
-		 }
-		
+		if(m_bodyA->isMultiDof())
+			m_jacSizeA = (6 + m_bodyA->getNumDofs());
+		else
+			m_jacSizeA = (6 + m_bodyA->getNumLinks());
 	}
 
-	
-	//compute rhs and remaining constraintRow fields
-
-	
-
-
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
+	if(m_bodyB)
 	{
+		if(m_bodyB->isMultiDof())
+			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
+		else
+			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
+	}
+	else
+		m_jacSizeBoth = m_jacSizeA;
+}
 
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} 
-		if (multiBodyB)
-		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
-			btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		}
-
-		constraintRow.m_friction = 0.f;
-
-		constraintRow.m_appliedImpulse = 0.f;
-		constraintRow.m_appliedPushImpulse = 0.f;
-		
-		btScalar	velocityError =  desiredVelocity - rel_vel;// * damping;
-
-		btScalar erp = infoGlobal.m_erp2;
-
-		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
-
-		if (!infoGlobal.m_splitImpulse)
-		{
-			//combine position and velocity into rhs
-			constraintRow.m_rhs = velocityImpulse;
-			constraintRow.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			constraintRow.m_rhs = velocityImpulse;
-			constraintRow.m_rhsPenetration = 0.f;
-		}
-
-
-		constraintRow.m_cfm = 0.f;
-		constraintRow.m_lowerLimit = lowerLimit;
-		constraintRow.m_upperLimit = upperLimit;
+void btMultiBodyConstraint::allocateJacobiansMultiDof()
+{
+	updateJacobianSizes();
 
-	}
-	return rel_vel;
+	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
 }
 
+btMultiBodyConstraint::~btMultiBodyConstraint()
+{
+}
 
 void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
-	for (int i = 0; i < ndof; ++i) 
+	for (int i = 0; i < ndof; ++i)
 		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
 }
 
-
-void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
-																	btMultiBodyJacobianData& data,
-																 const btVector3& contactNormalOnB,
-																 const btVector3& posAworld, const btVector3& posBworld, 
-																 btScalar position,
-																 const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint,
+															btMultiBodyJacobianData& data,
+															btScalar* jacOrgA, btScalar* jacOrgB,
+															const btVector3& contactNormalOnB,
+															const btVector3& posAworld, const btVector3& posBworld,
+															btScalar posError,
+															const btContactSolverInfo& infoGlobal,
+															btScalar lowerLimit, btScalar upperLimit,
+															btScalar relaxation,
+															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-			
-	
-	btVector3 rel_pos1 = posAworld;
-	btVector3 rel_pos2 = posBworld;
+
 
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;
 	solverConstraint.m_linkB = m_linkB;
-	
 
 	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
 	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
 
-	const btVector3& pos1 = posAworld;
-	const btVector3& pos2 = posBworld;
-
 	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
 	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
 
 	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
 	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
+	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
 	if (bodyA)
-		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
-		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-
-	relaxation = 1.f;
+		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
 
 	if (multiBodyA)
 	{
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
+		if (solverConstraint.m_linkA<0)
+		{
+			rel_pos1 = posAworld - multiBodyA->getBasePos();
+		} else
+		{
+			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -271,16 +115,41 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
 		}
 
+		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+		//resize..
 		solverConstraint.m_jacAindex = data.m_jacobians.size();
 		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		//copy/determine
+		if(jacOrgA)
+		{
+			for (int i=0;i<ndofA;i++)
+				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+		}
+		else
+		{
+			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+			if(multiBodyA->isMultiDof())
+				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+		}
 
-		btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+		//resize..
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	} else
+		//determine..
+		if(multiBodyA->isMultiDof())
+			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormalOnB;
+	}
+	else //if(rb0)
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
 		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -290,7 +159,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 
 	if (multiBodyB)
 	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
+		if (solverConstraint.m_linkB<0)
+		{
+			rel_pos2 = posBworld - multiBodyB->getBasePos();
+		} else
+		{
+			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -300,144 +177,142 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
 		}
 
+		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+		//resize..
 		solverConstraint.m_jacBindex = data.m_jacobians.size();
-
 		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		//copy/determine..
+		if(jacOrgB)
+		{
+			for (int i=0;i<ndofB;i++)
+				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+		}
+		else
+		{
+			if(multiBodyB->isMultiDof())
+				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+		}
+
+		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+		//resize..
 		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+		//determine..
+		if(multiBodyB->isMultiDof())
+			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormalOnB;
 
-		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
-	} else
+	}
+	else //if(rb1)
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
 		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormalOnB;
 	}
-
 	{
-						
+
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
 		btScalar* jacB = 0;
 		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
+		btScalar* deltaVelA = 0;
+		btScalar* deltaVelB = 0;
 		int ndofA  = 0;
+		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
 				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
+				btScalar l = deltaVelA[i];
 				denom0 += j*l;
 			}
-		} else
+		}
+		else if(rb0)
 		{
-			if (rb0)
-			{
-				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-				denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
-			}
+			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
 		}
+		//
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
 				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
+				btScalar l = deltaVelB[i];
 				denom1 += j*l;
 			}
 
-		} else
+		}
+		else if(rb1)
 		{
-			if (rb1)
-			{
-				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-				denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
-			}
+			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
 		}
 
-		 if (multiBodyA && (multiBodyA==multiBodyB))
-		 {
-            // ndof1 == ndof2 in this case
-            for (int i = 0; i < ndofA; ++i) 
-			{
-                denom1 += jacB[i] * lambdaA[i];
-                denom1 += jacA[i] * lambdaB[i];
-            }
-        }
-
-		 btScalar d = denom0+denom1;
-		 if (btFabs(d)>SIMD_EPSILON)
-		 {
-			 
-			 solverConstraint.m_jacDiagABInv = relaxation/(d);
-		 } else
-		 {
-			solverConstraint.m_jacDiagABInv  = 1.f;
-		 }
-		
+		//
+		btScalar d = denom0+denom1;
+		if (d>SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation/(d);
+		}
+		else
+		{
+		//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv  = 0.f;
+		}
 	}
 
-	
-	//compute rhs and remaining solverConstraint fields
 
-	
-
-	btScalar restitution = 0.f;
-	btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
+	//compute rhs and remaining solverConstraint fields
+	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
 
 	btScalar rel_vel = 0.f;
 	int ndofA  = 0;
 	int ndofB  = 0;
 	{
-
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
+			for (int i = 0; i < ndofA ; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} else
+		}
+		else if(rb0)
 		{
-			if (rb0)
-			{
-				rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
-			}
+			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
+			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
+			for (int i = 0; i < ndofB ; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 
-		} else
+		}
+		else if(rb1)
 		{
-			if (rb1)
-			{
-				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
-			}
+			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
 		}
 
 		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-
-				
-		restitution =  restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
-		if (restitution <= btScalar(0.))
-		{
-			restitution = 0.f;
-		};
 	}
 
 
@@ -474,18 +349,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		}
 	} else
 	*/
-	{
-		solverConstraint.m_appliedImpulse = 0.f;
-	}
 
+	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
-		
 
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
-					
+		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
+
 
 		btScalar erp = infoGlobal.m_erp2;
 		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -493,15 +365,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			erp = infoGlobal.m_erp;
 		}
 
-		if (penetration>0)
-		{
-			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
-
-		} else
-		{
-			positionalError = -penetration * erp/infoGlobal.m_timeStep;
-		}
+		positionalError = -penetration * erp/infoGlobal.m_timeStep;
 
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
@@ -520,8 +384,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		}
 
 		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
-		solverConstraint.m_upperLimit = m_maxAppliedImpulse;
+		solverConstraint.m_lowerLimit = lowerLimit;
+		solverConstraint.m_upperLimit = upperLimit;
 	}
 
+	return rel_vel;
+
 }

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -28,8 +28,8 @@ struct btSolverInfo;
 struct btMultiBodyJacobianData
 {
 	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;
+	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
+	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
 	btAlignedObjectArray<btScalar>		scratch_r;
 	btAlignedObjectArray<btVector3>		scratch_v;
 	btAlignedObjectArray<btMatrix3x3>	scratch_m;
@@ -48,16 +48,17 @@ protected:
     int				m_linkA;
     int				m_linkB;
 
-    int				m_num_rows;
-    int				m_jac_size_A;
-    int				m_jac_size_both;
-    int				m_pos_offset;
+    int				m_numRows;
+    int				m_jacSizeA;
+    int				m_jacSizeBoth;
+    int				m_posOffset;
 
 	bool			m_isUnilateral;
-
+	int				m_numDofsFinalized;
 	btScalar		m_maxAppliedImpulse;
 
 
+    // warning: the data block lay out is not consistent for all constraints
     // data block laid out as follows:
     // cached impulses. (one per row.)
     // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -66,40 +67,37 @@ protected:
 
 	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
 
-	void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
-																	btMultiBodyJacobianData& data,
-																 const btVector3& contactNormalOnB,
-																 const btVector3& posAworld, const btVector3& posBworld, 
-																 btScalar position,
-																 const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-		btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
-														btMultiBodyJacobianData& data,
-														btScalar* jacOrgA,btScalar* jacOrgB,
-														const btContactSolverInfo& infoGlobal,
-														btScalar desiredVelocity,
-														btScalar lowerLimit,
-														btScalar upperLimit);
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
+																btMultiBodyJacobianData& data,
+																btScalar* jacOrgA, btScalar* jacOrgB,
+																const btVector3& contactNormalOnB,
+																const btVector3& posAworld, const btVector3& posBworld,
+																btScalar posError,
+																const btContactSolverInfo& infoGlobal,
+																btScalar lowerLimit, btScalar upperLimit,
+																btScalar relaxation = 1.f,
+																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
 
 public:
 
 	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
 	virtual ~btMultiBodyConstraint();
 
+	void updateJacobianSizes();
+	void allocateJacobiansMultiDof();
 
+	virtual void finalizeMultiDof()=0;
 
 	virtual int getIslandIdA() const =0;
 	virtual int getIslandIdB() const =0;
-	
+
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal)=0;
 
 	int	getNumRows() const
 	{
-		return m_num_rows;
+		return m_numRows;
 	}
 
 	btMultiBody*	getMultiBodyA()
@@ -111,20 +109,33 @@ public:
 		return m_bodyB;
 	}
 
+	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		m_data[dof] = appliedImpulse;
+	}
+	
+	btScalar	getAppliedImpulse(int dof)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		return m_data[dof];
+	}
 	// current constraint position
     // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
     // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const 
-	{ 
-		return m_data[m_pos_offset + row]; 
+    btScalar getPosition(int row) const
+	{
+		return m_data[m_posOffset + row];
 	}
 
-    void setPosition(int row, btScalar pos) 
-	{ 
-		m_data[m_pos_offset + row] = pos; 
+    void setPosition(int row, btScalar pos)
+	{
+		m_data[m_posOffset + row] = pos;
 	}
 
-	
+
 	bool isUnilateral() const
 	{
 		return m_isUnilateral;
@@ -133,21 +144,21 @@ public:
 	// jacobian blocks.
     // each of size 6 + num_links. (jacobian2 is null if no body2.)
     // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row) 
-	{ 
-		return &m_data[m_num_rows + row * m_jac_size_both]; 
+    btScalar* jacobianA(int row)
+	{
+		return &m_data[m_numRows + row * m_jacSizeBoth];
 	}
-    const btScalar* jacobianA(int row) const 
-	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both)]; 
+    const btScalar* jacobianA(int row) const
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth)];
 	}
-    btScalar* jacobianB(int row) 
-	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+    btScalar* jacobianB(int row)
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
-    const btScalar* jacobianB(int row) const 
-	{ 
-		return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; 
+    const btScalar* jacobianB(int row) const
+	{
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
 
 	btScalar	getMaxAppliedImpulse() const
@@ -158,7 +169,8 @@ public:
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
-	
+
+	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -13,6 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+// Modified by Lasse Oorni for Urho3D
+
 #include "btMultiBodyConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "btMultiBodyLinkCollider.h"
@@ -36,6 +38,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		//if (iteration < constraint.m_overrideNumSolverIterations)
 			//resolveSingleConstraintRowGenericMultiBody(constraint);
 		resolveSingleConstraintRowGeneric(constraint);
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->setPosUpdated(false);
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->setPosUpdated(false);
 	}
 
 	//solve featherstone normal contact
@@ -44,6 +50,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
 		if (iteration < infoGlobal.m_numIterations)
 			resolveSingleConstraintRowGeneric(constraint);
+
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->setPosUpdated(false);
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->setPosUpdated(false);
 	}
 	
 	//solve featherstone frictional contact
@@ -60,6 +71,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
 				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
 				resolveSingleConstraintRowGeneric(frictionConstraint);
+
+				if(frictionConstraint.m_multiBodyA) 
+					frictionConstraint.m_multiBodyA->setPosUpdated(false);
+				if(frictionConstraint.m_multiBodyB) 
+					frictionConstraint.m_multiBodyB->setPosUpdated(false);
 			}
 		}
 	}
@@ -108,10 +124,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
 		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -119,10 +135,10 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyB)
 	{
-		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
 		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
@@ -151,8 +167,15 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	if (c.m_multiBodyA)
 	{
 		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
-		c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-	} else
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		if(c.m_multiBodyA->isMultiDof())
+			c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		else
+			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(c.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 
@@ -160,8 +183,15 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	if (c.m_multiBodyB)
 	{
 		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
-		c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-	} else
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		if(c.m_multiBodyB->isMultiDof())
+			c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		else
+			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	} else if(c.m_solverBodyIdB >= 0)
 	{
 		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 	}
@@ -180,14 +210,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
+		ndofA  = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
 		for (int i = 0; i < ndofA; ++i) 
 			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB = c.m_multiBodyB->getNumLinks() + 6;
+		ndofB  = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
 		for (int i = 0; i < ndofB; ++i) 
 			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	}
@@ -255,9 +285,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 	relaxation = 1.f;
 
+	
+
+
 	if (multiBodyA)
 	{
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
+		if (solverConstraint.m_linkA<0)
+		{
+			rel_pos1 = pos1 - multiBodyA->getBasePos();
+		} else
+		{
+			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+		}
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
@@ -277,20 +317,40 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
 		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		if(multiBodyA->isMultiDof())
+			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		else
+			multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		if(multiBodyA->isMultiDof())
+			multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		else
+			multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormal;
 	} else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
 	}
 
+	
+
 	if (multiBodyB)
 	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
+		if (solverConstraint.m_linkB<0)
+		{
+			rel_pos2 = pos2 - multiBodyB->getBasePos();
+		} else
+		{
+			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
@@ -306,14 +366,26 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+		if(multiBodyB->isMultiDof())
+			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		else
+			multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		if(multiBodyB->isMultiDof())
+			multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+		else
+			multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+		
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormal;
+	
 	} else
 	{
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
+	
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 	}
 
 	{
@@ -328,7 +400,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		int ndofA  = 0;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
@@ -347,7 +419,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
@@ -366,24 +438,16 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			}
 		}
 
-		 if (multiBodyA && (multiBodyA==multiBodyB))
-		 {
-            // ndof1 == ndof2 in this case
-            for (int i = 0; i < ndofA; ++i) 
-			{
-                denom1 += jacB[i] * lambdaA[i];
-                denom1 += jacA[i] * lambdaB[i];
-            }
-        }
+		 
 
 		 btScalar d = denom0+denom1;
-		 if (btFabs(d)>SIMD_EPSILON)
+		 if (d>SIMD_EPSILON)
 		 {
-			 
-			 solverConstraint.m_jacDiagABInv = relaxation/(d);
+			solverConstraint.m_jacDiagABInv = relaxation/(d);
 		 } else
 		 {
-			solverConstraint.m_jacDiagABInv  = 1.f;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv  = 0.f;
 		 }
 		
 	}
@@ -404,7 +468,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
+			ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
@@ -417,7 +481,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
+			ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
@@ -432,17 +496,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-				
-		restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-		if (restitution <= btScalar(0.))
+		if(!isFriction)
 		{
-			restitution = 0.f;
-		};
+			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);	
+			if (restitution <= btScalar(0.))
+			{
+				restitution = 0.f;
+			}
+		}
 	}
 
 
 	///warm starting (or zero if disabled)
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
+	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 	{
 		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 
@@ -452,7 +519,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
+				if(multiBodyA->isMultiDof())
+					multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
+				else
+					multiBodyA->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
 			} else
 			{
@@ -463,7 +533,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
+				if(multiBodyB->isMultiDof())
+					multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
+				else
+					multiBodyB->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
 			} else
 			{
@@ -479,11 +552,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
-		
 
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
-					
+		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 
 		btScalar erp = infoGlobal.m_erp2;
 		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -494,7 +565,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (penetration>0)
 		{
 			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
+			velocityError -= penetration / infoGlobal.m_timeStep;
 
 		} else
 		{
@@ -504,22 +575,33 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		if(!isFriction)
 		{
-			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
+			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
+			} else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				solverConstraint.m_rhs = velocityImpulse;
+				solverConstraint.m_rhsPenetration = penetrationImpulse;
+			}
+
+			solverConstraint.m_lowerLimit = 0;
+			solverConstraint.m_upperLimit = 1e10f;
+		}
+		else
 		{
-			//split position and velocity into rhs and m_rhsPenetration
 			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+			solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		}
 
-		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
 	}
 
 }
@@ -531,6 +613,9 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
+    solverConstraint.m_orgConstraint = 0;
+    solverConstraint.m_orgDofIndex = -1;
+    
 	solverConstraint.m_frictionIndex = frictionIndex;
 	bool isFriction = true;
 
@@ -575,15 +660,15 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
 	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
 
-	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
+//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
+//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
 
 
 	///avoid collision response between two static objects
 //	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
 	//	return;
 
-	int rollingFriction=1;
+
 
 	for (int j=0;j<manifold->getNumContacts();j++)
 	{
@@ -599,8 +684,10 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
 
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+            solverConstraint.m_orgConstraint = 0;
+            solverConstraint.m_orgDofIndex = -1;
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -624,6 +711,7 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 #ifdef ENABLE_FRICTION
 			solverConstraint.m_frictionIndex = frictionIndex;
 #if ROLLING_FRICTION
+	int rollingFriction=1;
 			btVector3 angVelA(0,0,0),angVelB(0,0,0);
 			if (rb0)
 				angVelA = rb0->getAngularVelocity();
@@ -702,6 +790,10 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				{
 					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -709,10 +801,6 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
 					}
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
-
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
 						cp.m_lateralFrictionInitialized = true;
@@ -741,7 +829,7 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	btPersistentManifold* manifold = 0;
+	//btPersistentManifold* manifold = 0;
 
 	for (int i=0;i<numManifolds;i++)
 	{
@@ -779,6 +867,272 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
+#if 0
+static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
+{
+	if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
+	{
+		//todo: get rid of those temporary memory allocations for the joint feedback
+		btAlignedObjectArray<btScalar> forceVector;
+		int numDofsPlusBase = 6+mb->getNumDofs();
+		forceVector.resize(numDofsPlusBase);
+		for (int i=0;i<numDofsPlusBase;i++)
+		{
+			forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
+		}
+		btAlignedObjectArray<btScalar> output;
+		output.resize(numDofsPlusBase);
+		bool applyJointFeedback = true;
+		mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
+	}
+}
+#endif
+
+// Urho3D: disable include
+//#include "Bullet3Common/b3Logging.h"
+void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
+{
+#if 1 
+	
+	//bod->addBaseForce(m_gravity * bod->getBaseMass());
+	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+
+	if (c.m_orgConstraint)
+	{
+		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
+	}
+	
+
+	if (c.m_multiBodyA)
+	{
+		
+		if(c.m_multiBodyA->isMultiDof())
+		{
+			c.m_multiBodyA->setCompanionId(-1);
+			btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
+			btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
+			if (c.m_linkA<0)
+			{
+				c.m_multiBodyA->addBaseConstraintForce(force);
+				c.m_multiBodyA->addBaseConstraintTorque(torque);
+			} else
+			{
+				c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
+					//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
+				c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
+			}
+		}
+	}
+	
+	if (c.m_multiBodyB)
+	{
+		if(c.m_multiBodyB->isMultiDof())
+		{
+			{
+				c.m_multiBodyB->setCompanionId(-1);
+				btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
+				btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
+				if (c.m_linkB<0)
+				{
+					c.m_multiBodyB->addBaseConstraintForce(force);
+					c.m_multiBodyB->addBaseConstraintTorque(torque);
+				} else
+				{
+					{
+						c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
+						//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
+						c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
+					}
+					
+				}
+			}
+		}
+	}
+#endif
+
+#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+	if (c.m_multiBodyA)
+	{
+		
+		if(c.m_multiBodyA->isMultiDof())
+		{
+			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
+		}
+		else
+		{
+			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
+		}
+	}
+	
+	if (c.m_multiBodyB)
+	{
+		if(c.m_multiBodyB->isMultiDof())
+		{
+			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
+		}
+		else
+		{
+			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
+		}
+	}
+#endif
+
+
+
+}
+
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
+	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
+	
+
+	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
+	//or as applied force, so we can measure the joint reaction forces easier
+	for (int i=0;i<numPoolConstraints;i++)
+	{
+		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
+		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+
+		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
+
+		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+		{
+			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
+		}
+	}
+
+
+	for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
+	{
+		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+	}
+
+	
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		BT_PROFILE("warm starting write back");
+		for (int j=0;j<numPoolConstraints;j++)
+		{
+			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
+			btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
+			btAssert(pt);
+			pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
+			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
+			
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
+			}
+			//do a callback here?
+		}
+	}
+#if 0
+	//multibody joint feedback
+	{
+		BT_PROFILE("multi body joint feedback");
+		for (int j=0;j<numPoolConstraints;j++)
+		{
+			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
+		
+			//apply the joint feedback into all links of the btMultiBody
+			//todo: double-check the signs of the applied impulse
+
+			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
+			{
+				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
+			}
+			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
+			{
+				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
+			}
+#if 0
+			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
+			{
+				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
+
+			}
+			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
+			{
+				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
+					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
+			}
+		
+			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
+				{
+					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
+				}
+
+				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
+				{
+					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
+						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
+				}
+			}
+#endif
+		}
+	
+		for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
+		{
+			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
+			{
+				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
+			}
+			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
+			{
+				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
+			}
+		}
+	}
+
+	numPoolConstraints = m_multiBodyNonContactConstraints.size();
+
+#if 0
+	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
+	for (int i=0;i<numPoolConstraints;i++)
+	{
+		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
+
+		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
+		btJointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
+			
+		}
+
+		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
+		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		{
+			constr->setEnabled(false);
+		}
+
+	}
+#endif 
+#endif
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
+}
+
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
 #include "btMultiBodySolverConstraint.h"
 
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 
 class btMultiBody;
 
@@ -66,14 +67,15 @@ protected:
 
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
-
+	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	
 	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
 };
 

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -20,8 +20,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 #include "LinearMath/btQuickprof.h"
 #include "btMultiBodyConstraint.h"
-
-	
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btSerializer.h"
 
 
 void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
@@ -364,7 +364,9 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
 		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
 		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;			
+
+		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 	
 		m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
@@ -392,11 +394,20 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 	delete m_solverMultiBodyIslandCallback;
 }
 
+void	btMultiBodyDynamicsWorld::forwardKinematics()
+{
+	btAlignedObjectArray<btQuaternion> world_to_local;
+	btAlignedObjectArray<btVector3> local_origin;
 
-
-
+	for (int b=0;b<m_multiBodies.size();b++)
+	{
+		btMultiBody* bod = m_multiBodies[b];
+		bod->forwardKinematics(world_to_local,local_origin);
+	}
+}
 void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
+	forwardKinematics();
 
 	btAlignedObjectArray<btScalar> scratch_r;
 	btAlignedObjectArray<btVector3> scratch_v;
@@ -430,9 +441,9 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
 
-
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	{
-		BT_PROFILE("btMultiBody addForce and stepVelocities");
+		BT_PROFILE("btMultiBody addForce");
 		for (int i=0;i<this->m_multiBodies.size();i++)
 		{
 			btMultiBody* bod = m_multiBodies[i];
@@ -451,27 +462,271 @@ void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 			if (!isSleeping)
 			{
-				scratch_r.resize(bod->getNumLinks()+1);
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
 				scratch_v.resize(bod->getNumLinks()+1);
 				scratch_m.resize(bod->getNumLinks()+1);
 
-				bod->clearForcesAndTorques();
 				bod->addBaseForce(m_gravity * bod->getBaseMass());
 
 				for (int j = 0; j < bod->getNumLinks(); ++j) 
 				{
 					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 				}
+			}//if (!isSleeping)
+		}
+	}
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+	
 
-				bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
-			}
+	{
+		BT_PROFILE("btMultiBody stepVelocities");
+		for (int i=0;i<this->m_multiBodies.size();i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
+
+			bool isSleeping = false;
+			
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			} 
+			for (int b=0;b<bod->getNumLinks();b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+					isSleeping = true;
+			} 
+
+			if (!isSleeping)
+			{
+				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
+				scratch_v.resize(bod->getNumLinks()+1);
+				scratch_m.resize(bod->getNumLinks()+1);
+				bool doNotUpdatePos = false;
+
+				if(bod->isMultiDof())
+				{
+					if(!bod->isUsingRK4Integration())
+					{
+						bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+					}
+					else
+					{						
+						//
+						int numDofs = bod->getNumDofs() + 6;
+						int numPosVars = bod->getNumPosVars() + 7;
+						btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
+						//convenience
+						btScalar *pMem = &scratch_r2[0];
+						btScalar *scratch_q0 = pMem; pMem += numPosVars;
+						btScalar *scratch_qx = pMem; pMem += numPosVars;
+						btScalar *scratch_qd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qd3 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd0 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd1 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd2 = pMem; pMem += numDofs;
+						btScalar *scratch_qdd3 = pMem; pMem += numDofs;
+						btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
+
+						/////						
+						//copy q0 to scratch_q0 and qd0 to scratch_qd0
+						scratch_q0[0] = bod->getWorldToBaseRot().x();
+						scratch_q0[1] = bod->getWorldToBaseRot().y();
+						scratch_q0[2] = bod->getWorldToBaseRot().z();
+						scratch_q0[3] = bod->getWorldToBaseRot().w();
+						scratch_q0[4] = bod->getBasePos().x();
+						scratch_q0[5] = bod->getBasePos().y();
+						scratch_q0[6] = bod->getBasePos().z();
+						//
+						for(int link = 0; link < bod->getNumLinks(); ++link)
+						{
+							for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];							
+						}
+						//
+						for(int dof = 0; dof < numDofs; ++dof)								
+							scratch_qd0[dof] = bod->getVelocityVector()[dof];
+						////
+						struct
+						{
+						    btMultiBody *bod;
+                            btScalar *scratch_qx, *scratch_q0;
+
+						    void operator()()
+						    {
+						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+						    }
+						} pResetQx = {bod, scratch_qx, scratch_q0};
+						//
+						struct
+						{
+						    void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+						    }
+
+						} pEulerIntegrate;
+						//
+						struct
+                        {
+                            void operator()(btMultiBody *pBody, const btScalar *pData)
+                            {
+                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+
+                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+
+                            }
+                        } pCopyToVelocityVector;
+						//
+                        struct
+						{
+						    void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
+						    {
+						        for(int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+						    }
+						} pCopy;
+						//
+
+						btScalar h = solverInfo.m_timeStep;
+						#define output &scratch_r[bod->getNumDofs()]
+						//calc qdd0 from: q0 & qd0	
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd0, 0, numDofs);
+						//calc q1 = q0 + h/2 * qd0
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
+						//calc qd1 = qd0 + h/2 * qdd0
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+						//
+						//calc qdd1 from: q1 & qd1
+						pCopyToVelocityVector(bod, scratch_qd1);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd1, 0, numDofs);
+						//calc q2 = q0 + h/2 * qd1
+						pResetQx();
+						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
+						//calc qd2 = qd0 + h/2 * qdd1
+						pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+						//
+						//calc qdd2 from: q2 & qd2
+						pCopyToVelocityVector(bod, scratch_qd2);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd2, 0, numDofs);
+						//calc q3 = q0 + h * qd2
+						pResetQx();
+						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+						//calc qd3 = qd0 + h * qdd2
+						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+						//
+						//calc qdd3 from: q3 & qd3
+						pCopyToVelocityVector(bod, scratch_qd3);
+						bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
+						pCopy(output, scratch_qdd3, 0, numDofs);
+
+						//
+						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)						
+						btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
+						btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
+						for(int i = 0; i < numDofs; ++i)
+						{
+							delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
+							delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);							
+							//delta_q[i] = h*scratch_qd0[i];
+							//delta_qd[i] = h*scratch_qdd0[i];
+						}
+						//
+						pCopyToVelocityVector(bod, scratch_qd0);
+						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);						
+						//
+						if(!doNotUpdatePos)
+						{
+							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+							for(int i = 0; i < numDofs; ++i)
+								pRealBuf[i] = delta_q[i];
+
+							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+							bod->setPosUpdated(true);							
+						}
+
+						//ugly hack which resets the cached data to t0 (needed for constraint solver)
+						{
+							for(int link = 0; link < bod->getNumLinks(); ++link)
+								bod->getLink(link).updateCacheMultiDof();
+							bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
+						}
+						
+					}
+				}
+				else//if(bod->isMultiDof())
+				{
+					bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+				}
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+				bod->clearForcesAndTorques();
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+			}//if (!isSleeping)
 		}
 	}
 
+	clearMultiBodyConstraintForces();
+
 	m_solverMultiBodyIslandCallback->processConstraints();
 	
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 
+	{
+                BT_PROFILE("btMultiBody stepVelocities");
+                for (int i=0;i<this->m_multiBodies.size();i++)
+                {
+                        btMultiBody* bod = m_multiBodies[i];
+
+                        bool isSleeping = false;
+
+                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                        {
+                                isSleeping = true;
+                        }
+                        for (int b=0;b<bod->getNumLinks();b++)
+                        {
+                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+                                        isSleeping = true;
+                        }
+
+                        if (!isSleeping)
+                        {
+                                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                                scratch_r.resize(bod->getNumLinks()+1);                 //multidof? ("Y"s use it and it is used to store qdd)
+                                scratch_v.resize(bod->getNumLinks()+1);
+                                scratch_m.resize(bod->getNumLinks()+1);
+
+                                if(bod->isMultiDof())
+                                {
+                                        if(!bod->isUsingRK4Integration())
+                                        {
+						bool isConstraintPass = true;
+                                                bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
+                                        }
+				}
+			}
+		}
+	}
+
+	for (int i=0;i<this->m_multiBodies.size();i++)
+       {
+                btMultiBody* bod = m_multiBodies[i];
+		bod->processDeltaVeeMultiDof2();
+	}
+
 }
 
 void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
@@ -503,76 +758,248 @@ void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 			{
 				int nLinks = bod->getNumLinks();
 
-				///base + num links
+				///base + num m_links
+			
+				if(bod->isMultiDof())
+				{
+					if(!bod->isPosUpdated())
+						bod->stepPositionsMultiDof(timeStep);
+					else
+					{
+						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
+						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+
+						bod->stepPositionsMultiDof(1, 0, pRealBuf);
+						bod->setPosUpdated(false);
+					}
+				}
+				else
+				{
+					bod->stepPositions(timeStep);			
+				}
 				world_to_local.resize(nLinks+1);
 				local_origin.resize(nLinks+1);
 
-				bod->stepPositions(timeStep);
+				bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
+				
+			} else
+			{
+				bod->clearVelocities();
+			}
+		}
+	}
+}
 
-	 
 
-				world_to_local[0] = bod->getWorldToBaseRot();
-				local_origin[0] = bod->getBasePos();
 
-				if (bod->getBaseCollider())
-				{
-					btVector3 posr = local_origin[0];
-					float pos[4]={posr.x(),posr.y(),posr.z(),1};
-					float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
-					btTransform tr;
-					tr.setIdentity();
-					tr.setOrigin(posr);
-					tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+void	btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.push_back(constraint);
+}
 
-					bod->getBaseCollider()->setWorldTransform(tr);
+void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+{
+	m_multiBodyConstraints.remove(constraint);
+}
 
-				}
-      
-				for (int k=0;k<bod->getNumLinks();k++)
-				{
-					const int parent = bod->getParent(k);
-					world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
-					local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
-				}
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	constraint->debugDraw(getDebugDrawer());
+}
 
 
-				for (int m=0;m<bod->getNumLinks();m++)
-				{
-					btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
-					if (col)
-					{
-						int link = col->m_link;
-						btAssert(link == m);
+void	btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
 
-						int index = link+1;
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
 
-						btVector3 posr = local_origin[index];
-						float pos[4]={posr.x(),posr.y(),posr.z(),1};
-						float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
-						btTransform tr;
-						tr.setIdentity();
-						tr.setOrigin(posr);
-						tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+		if (drawConstraints)
+		{
+			BT_PROFILE("btMultiBody debugDrawWorld");
+			
+			btAlignedObjectArray<btQuaternion> world_to_local1;
+			btAlignedObjectArray<btVector3> local_origin1;
 
-						col->setWorldTransform(tr);
+			for (int c=0;c<m_multiBodyConstraints.size();c++)
+			{
+				btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
+				debugDrawMultiBodyConstraint(constraint);
+			}
+
+			for (int b = 0; b<m_multiBodies.size(); b++)
+			{
+				btMultiBody* bod = m_multiBodies[b];
+				bod->forwardKinematics(world_to_local1,local_origin1);
+				
+				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+
+
+				for (int m = 0; m<bod->getNumLinks(); m++)
+				{
+					
+					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+
+						//draw the joint axis
+					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
+					
+						btVector4 color(0,0,0,1);//1,1,1);
+						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from,to,color);
+					}
+					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+					
+						btVector4 color(0,0,0,1);//1,1,1);
+						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from,to,color);
+					}
+					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
+					
+						btVector4 color(0,0,0,1);//1,1,1);
+						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from,to,color);
 					}
+					
 				}
-			} else
-			{
-				bod->clearVelocities();
 			}
 		}
 	}
+
+	btDiscreteDynamicsWorld::debugDrawWorld();
 }
 
 
 
-void	btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::applyGravity()
 {
-	m_multiBodyConstraints.push_back(constraint);
+        btDiscreteDynamicsWorld::applyGravity();
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+        BT_PROFILE("btMultiBody addGravity");
+        for (int i=0;i<this->m_multiBodies.size();i++)
+        {
+                btMultiBody* bod = m_multiBodies[i];
+
+                bool isSleeping = false;
+
+                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                {
+                        isSleeping = true;
+                }
+                for (int b=0;b<bod->getNumLinks();b++)
+                {
+                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+                                isSleeping = true;
+                }
+
+                if (!isSleeping)
+                {
+                        bod->addBaseForce(m_gravity * bod->getBaseMass());
+
+                        for (int j = 0; j < bod->getNumLinks(); ++j)
+                        {
+                                bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                        }
+                }//if (!isSleeping)
+        }
+#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 }
 
-void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
+{ 
+  for (int i=0;i<this->m_multiBodies.size();i++)
+                {       
+                        btMultiBody* bod = m_multiBodies[i];
+			bod->clearConstraintForces();
+                  } 
+}
+void btMultiBodyDynamicsWorld::clearMultiBodyForces()
 {
-	m_multiBodyConstraints.remove(constraint);
+              {
+                BT_PROFILE("clearMultiBodyForces");
+                for (int i=0;i<this->m_multiBodies.size();i++)
+                {
+                        btMultiBody* bod = m_multiBodies[i];
+
+                        bool isSleeping = false;
+
+                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                        {       
+                                isSleeping = true;
+                        }
+                        for (int b=0;b<bod->getNumLinks();b++)
+                        {       
+                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)     
+                                        isSleeping = true;
+                        }
+
+                        if (!isSleeping)
+                        {
+                        btMultiBody* bod = m_multiBodies[i];
+                        bod->clearForcesAndTorques();
+                	}
+		}
+	}
+
 }
+void btMultiBodyDynamicsWorld::clearForces()
+{
+        btDiscreteDynamicsWorld::clearForces();
+
+#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+	clearMultiBodyForces();
+#endif
+}
+
+
+
+
+void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo( serializer);
+
+	serializeMultiBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializer->finishSerialization();
+}
+
+void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i=0;i<m_multiBodies.size();i++)
+	{
+		btMultiBody* mb = m_multiBodies[i];
+		{
+			int len = mb->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+		}
+	}
+
+}

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 
+#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 
 class btMultiBody;
 class btMultiBodyConstraint;
@@ -38,19 +39,61 @@ protected:
 	virtual void	calculateSimulationIslands();
 	virtual void	updateActivationState(btScalar timeStep);
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	virtual void	integrateTransforms(btScalar timeStep);
+	
+	virtual void	serializeMultiBodies(btSerializer* serializer);
+
 public:
 
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
-	
+
 	virtual ~btMultiBodyDynamicsWorld ();
 
 	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
 
 	virtual void	removeMultiBody(btMultiBody* body);
 
+	virtual int		getNumMultibodies() const
+	{
+		return m_multiBodies.size();
+	}
+
+	btMultiBody*	getMultiBody(int mbIndex)
+	{
+		return m_multiBodies[mbIndex];
+	}
+
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
+	virtual int     getNumMultiBodyConstraints() const
+	{
+        return m_multiBodyConstraints.size();
+	}
+
+	virtual btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex)
+	{
+        return m_multiBodyConstraints[constraintIndex];
+	}
+
+	virtual const btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex) const
+	{
+        return m_multiBodyConstraints[constraintIndex];
+	}
+
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	virtual void	integrateTransforms(btScalar timeStep);
+
+	virtual void	debugDrawWorld();
+
+	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
+	
+	void	forwardKinematics();
+	virtual void clearForces();
+	virtual void clearMultiBodyConstraintForces();
+	virtual void clearMultiBodyForces();
+	virtual void applyGravity();
+	
+	virtual	void	serialize(btSerializer* serializer);
+
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

Source/ThirdParty/Bullet/BulletLicense.txt → Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2015 Google 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.
@@ -13,6 +13,15 @@ subject to the following restrictions:
 */
 
 
-Free for commercial use, please report projects in the forum at http://www.bulletphysics.org
 
-In case you want to display a Bullet logo in your software: you can download the Bullet logo in various vector formats and high resolution at the download section in http://bullet.googlecode.com
+#ifndef BT_MULTIBODY_JOINT_FEEDBACK_H
+#define BT_MULTIBODY_JOINT_FEEDBACK_H
+
+#include "LinearMath/btSpatialAlgebra.h"
+
+struct btMultiBodyJointFeedback
+{
+	btSpatialForceVector	m_reactionForces;
+};
+
+#endif //BT_MULTIBODY_JOINT_FEEDBACK_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -21,51 +21,68 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 
+
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
-	:btMultiBodyConstraint(body,body,link,link,2,true),
+	//:btMultiBodyConstraint(body,0,link,-1,2,true),
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
 	m_lowerBound(lower),
 	m_upperBound(upper)
+{
+
+}
+
+void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 {
 	// the data.m_jacobians never change, so may as well
     // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
 
-    // row 0: the lower bound
-    jacobianA(0)[6 + link] = 1;
+	allocateJacobiansMultiDof();
 
-    // row 1: the upper bound
-    jacobianB(1)[6 + link] = -1;
+	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA);
+
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+	// row 1: the upper bound
+	//jacobianA(1)[offset] = -1;
+	jacobianB(1)[offset] = -1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
 }
+
 btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
 
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	if(m_bodyA)
 	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		{
+			if (m_bodyA->getLink(i).m_collider)
+				return m_bodyA->getLink(i).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	if(m_bodyB)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
+
+		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		{
+			col = m_bodyB->getLink(i).m_collider;
+			if (col)
+				return col->getIslandTag();
+		}
 	}
 	return -1;
 }
@@ -75,22 +92,72 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal)
 {
+	
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
-    
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+
     // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
+    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
 
     // row 1: the upper bound
     setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-
+	
 	for (int row=0;row<getNumRows();row++)
 	{
+		
+		btScalar direction = row? -1 : 1;
+
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+        constraintRow.m_orgConstraint = this;
+        constraintRow.m_orgDofIndex = row;
+        
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
-		
-		btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+		const btScalar posError = 0;						//why assume it's zero?
+		const btVector3 dummy(0, 0, 0);
+
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+
+		if (m_bodyA->isMultiDof())
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
+					
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1=prismaticAxisInWorld;
+					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+			
+		}
+
 		{
 			btScalar penetration = getPosition(row);
 			btScalar positionalError = 0.f;
@@ -127,7 +194,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 	}
 
 }
-	
-	
-	
+
+
+
 

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -30,14 +30,20 @@ public:
 	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
 	virtual ~btMultiBodyJointLimitConstraint();
 
+	virtual void finalizeMultiDof();
+
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
-	
-	
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+
 };
 
 #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -22,18 +22,41 @@ subject to the following restrictions:
 
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
-	:btMultiBodyConstraint(body,body,link,link,1,true),
-	m_desiredVelocity(desiredVelocity)	
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
+	m_desiredVelocity(desiredVelocity)
 {
+
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
     // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
 
-    // row 0: the lower bound
-    jacobianA(0)[6 + link] = 1;
+
+}
+
+void btMultiBodyJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	//:btMultiBodyConstraint(body,0,link,-1,1,true),
+	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
+	m_desiredVelocity(desiredVelocity)
+{
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+
 }
 btMultiBodyJointMotor::~btMultiBodyJointMotor()
 {
@@ -74,16 +97,62 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 {
     // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
-    
-  
+	
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+        finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
 
 	for (int row=0;row<getNumRows();row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-		
-		btScalar penetration = 0;
-		fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse);
+
+
+		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		if (m_bodyA->isMultiDof())
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
+					
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1=prismaticAxisInWorld;
+					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+			
+		}
+
 	}
 
 }
-	
+

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -25,13 +25,15 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
 {
 protected:
 
-	
+
 	btScalar	m_desiredVelocity;
 
 public:
 
 	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
+	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
+    virtual void finalizeMultiDof();
 
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
@@ -39,8 +41,16 @@ public:
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
-	
-	
+
+    virtual void setVelocityTarget(btScalar velTarget)
+    {
+        m_desiredVelocity = velTarget;
+    }
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
 };
 
 #endif //BT_MULTIBODY_JOINT_MOTOR_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -24,84 +24,207 @@ enum	btMultiBodyLinkFlags
 {
 	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
 };
+
+//both defines are now permanently enabled
+#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define TEST_SPATIAL_ALGEBRA_LAYER
+
 //
-// Link struct
+// Various spatial helper functions
 //
 
-struct btMultibodyLink 
-{
+//namespace {
 
-	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btScalar joint_pos;    // qi
+#include "LinearMath/btSpatialAlgebra.h"
 
-    btScalar mass;         // mass of link
-    btVector3 inertia;   // inertia of link (local frame; diagonal)
+//}
 
-    int parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+//
+// Link struct
+//
 
-    btQuaternion zero_rot_parent_to_this;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+struct btMultibodyLink 
+{
 
-    // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
-    // for prismatic: axis_top = zero;
-    //                axis_bottom = unit vector along the joint axis.
-    // for revolute: axis_top = unit vector along the rotation axis (u);
-    //               axis_bottom = u cross d_vector.
-    btVector3 axis_top;
-    btVector3 axis_bottom;
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btVector3 d_vector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
+    btScalar m_mass;         // mass of link
+    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
 
-    // e_vector is constant, but depends on the joint type
-    // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
-    // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
-    btVector3 e_vector;
+    int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
 
-    bool is_revolute;   // true = revolute, false = prismatic
+    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
 
-    btQuaternion cached_rot_parent_to_this;   // rotates vectors in parent frame to vectors in local frame
-    btVector3 cached_r_vector;                // vector from COM of parent to COM of this link, in local frame.
+    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant.
+	//this is set to zero for planar joint (see also m_eVector comment)
+	
+    // m_eVector is constant, but depends on the joint type:
+    // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
+	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
+	// todo: fix the planar so it is consistent with the other joints
+	
+    btVector3 m_eVector;
 
-    btVector3 applied_force;    // In WORLD frame
-    btVector3 applied_torque;   // In WORLD frame
-    btScalar joint_torque;
+	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
 
+	enum eFeatherstoneJointType
+	{
+		eRevolute = 0,
+		ePrismatic = 1,
+		eSpherical = 2,
+		ePlanar = 3,
+		eFixed = 4,
+		eInvalid
+	};
+
+	
+
+	// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
+    // for prismatic: m_axesTop[0] = zero;
+    //                m_axesBottom[0] = unit vector along the joint axis.
+    // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
+    //               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
+	//
+	// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
+	//				  m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
+	//
+	// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
+	//			   m_axesTop[1][2] = zero
+	//			   m_axesBottom[0] = zero
+	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane		
+	btSpatialMotionVector m_axes[6];
+	void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
+	void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
+	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
+	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
+	const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
+	const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
+
+	int m_dofOffset, m_cfgOffset;
+
+    btQuaternion m_cachedRotParentToThis;   // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector;                // vector from COM of parent to COM of this link, in local frame.
+
+    btVector3 m_appliedForce;    // In WORLD frame
+    btVector3 m_appliedTorque;   // In WORLD frame
+
+btVector3 m_appliedConstraintForce;    // In WORLD frame
+    btVector3 m_appliedConstraintTorque;   // In WORLD frame
+
+	btScalar m_jointPos[7];
+    
+    //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
+    //It gets set to zero after each internal stepSimulation call
+	btScalar m_jointTorque[6];
+    
 	class btMultiBodyLinkCollider* m_collider;
 	int m_flags;
+	
+	
+	int m_dofCount, m_posVarCount;				//redundant but handy
+	
+	eFeatherstoneJointType m_jointType;
+	
+	struct btMultiBodyJointFeedback*	m_jointFeedback;
+
+	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
+
+	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
+	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
 
     // ctor: set some sensible defaults
 	btMultibodyLink()
-		: joint_pos(0),
-			mass(1),
-			parent(-1),
-			zero_rot_parent_to_this(1, 0, 0, 0),
-			is_revolute(false),
-			cached_rot_parent_to_this(1, 0, 0, 0),
-			joint_torque(0),
+		: 	m_mass(1),
+			m_parent(-1),
+			m_zeroRotParentToThis(0, 0, 0, 1),
+			m_cachedRotParentToThis(0, 0, 0, 1),
 			m_collider(0),
-			m_flags(0)
+			m_flags(0),
+			m_dofCount(0),
+			m_posVarCount(0),
+			m_jointType(btMultibodyLink::eInvalid),
+			m_jointFeedback(0),
+			m_linkName(0),
+			m_jointName(0)
 	{
-		inertia.setValue(1, 1, 1);
-		axis_top.setValue(0, 0, 0);
-		axis_bottom.setValue(1, 0, 0);
-		d_vector.setValue(0, 0, 0);
-		e_vector.setValue(0, 0, 0);
-		cached_r_vector.setValue(0, 0, 0);
-		applied_force.setValue( 0, 0, 0);
-		applied_torque.setValue(0, 0, 0);
+		
+		m_inertiaLocal.setValue(1, 1, 1);
+		setAxisTop(0, 0., 0., 0.);
+		setAxisBottom(0, 1., 0., 0.);
+		m_dVector.setValue(0, 0, 0);
+		m_eVector.setValue(0, 0, 0);
+		m_cachedRVector.setValue(0, 0, 0);
+		m_appliedForce.setValue( 0, 0, 0);
+		m_appliedTorque.setValue(0, 0, 0);
+		//		
+		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
+		m_jointPos[3] = 1.f;			//"quat.w"
+		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
+		m_cachedWorldTransform.setIdentity();
 	}
 
-    // routine to update cached_rot_parent_to_this and cached_r_vector
+    // routine to update m_cachedRotParentToThis and m_cachedRVector
     void updateCache()
 	{
-		if (is_revolute) 
+		//multidof
+		if (m_jointType == eRevolute) 
 		{
-			cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
-			cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
+			m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
+			m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
 		} else 
 		{
-			// cached_rot_parent_to_this never changes, so no need to update
-			cached_r_vector = e_vector + joint_pos * axis_bottom;
+			// m_cachedRotParentToThis never changes, so no need to update
+			m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
+		}
+	}
+
+	void updateCacheMultiDof(btScalar *pq = 0)
+	{
+		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+
+		switch(m_jointType)
+		{
+			case eRevolute:
+			{
+				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+
+				break;
+			}
+			case ePrismatic:
+			{
+				// m_cachedRotParentToThis never changes, so no need to update
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
+
+				break;
+			}
+			case eSpherical:
+			{
+				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+
+				break;
+			}
+			case ePlanar:
+			{
+				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
+				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);				
+
+				break;
+			}
+			case eFixed:
+			{
+				m_cachedRotParentToThis = m_zeroRotParentToThis;
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+
+				break;
+			}
+			default:
+			{
+				//invalid type
+				btAssert(0);
+			}
 		}
 	}
 };

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -74,14 +74,14 @@ public:
 		if (m_link>=0)
 		{
 			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link)
+			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
 				return false;
 		}
 		
 		if (other->m_link>=0)
 		{
 			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
-			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link)
+			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
 				return false;
 		}
 		return true;

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -18,25 +18,39 @@ subject to the following restrictions:
 #include "btMultiBodyPoint2Point.h"
 #include "btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+	#define BTMBP2PCONSTRAINT_DIM 3
+#else
+	#define BTMBP2PCONSTRAINT_DIM 6
+#endif
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(body,0,link,-1,3,false),
+	:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
 	m_rigidBodyA(0),
 	m_rigidBodyB(bodyB),
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
+	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
 	m_rigidBodyA(0),
 	m_rigidBodyB(0),
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
+void btMultiBodyPoint2Point::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
 
 btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
 {
@@ -90,25 +104,37 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 {
 
 //	int i=1;
-	for (int i=0;i<3;i++)
+int numDim = BTMBP2PCONSTRAINT_DIM;
+	for (int i=0;i<numDim;i++)
 	{
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+	constraintRow.m_orgConstraint = this;
+	constraintRow.m_orgDofIndex = i;
+        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
+        constraintRow.m_contactNormal1.setValue(0,0,0);
+        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
+        constraintRow.m_contactNormal2.setValue(0,0,0);
+        constraintRow.m_angularComponentA.setValue(0,0,0);
+        constraintRow.m_angularComponentB.setValue(0,0,0);
 
 		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
 		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
-		
 
 		btVector3 contactNormalOnB(0,0,0);
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 		contactNormalOnB[i] = -1;
+#else
+		contactNormalOnB[i%3] = -1;
+#endif
 
-		btScalar penetration = 0;
 
 		 // Convert local points back to world
 		btVector3 pivotAworld = m_pivotInA;
 		if (m_rigidBodyA)
 		{
-			
+
 			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
 			pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
 		} else
@@ -125,19 +151,71 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		{
 			if (m_bodyB)
 				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
-			
+
 		}
-		btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
-		btScalar relaxation = 1.f;
-		fillMultiBodyConstraintMixed(constraintRow, data,
-																 contactNormalOnB,
-																 pivotAworld, pivotBworld, 
-																 position,
-																 infoGlobal,
-																 relaxation,
-																 false);
-		constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
-		constraintRow.m_upperLimit = m_maxAppliedImpulse;
 
+		btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
+
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
+
+		fillMultiBodyConstraint(constraintRow, data, 0, 0,
+															contactNormalOnB, pivotAworld, pivotBworld,						//sucks but let it be this way "for the time being"
+															posError,
+															infoGlobal,
+															-m_maxAppliedImpulse, m_maxAppliedImpulse
+															);
+    //@todo: support the case of btMultiBody versus btRigidBody,
+    //see btPoint2PointConstraint::getInfo2NonVirtual
+#else
+		const btVector3 dummy(0, 0, 0);
+
+		btAssert(m_bodyA->isMultiDof());
+
+		btScalar* jac1 = jacobianA(i);
+		const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
+		const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
+
+		m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+
+		fillMultiBodyConstraint(constraintRow, data, jac1, 0,
+													dummy, dummy, dummy,						//sucks but let it be this way "for the time being"
+													posError,
+													infoGlobal,
+													-m_maxAppliedImpulse, m_maxAppliedImpulse
+													);
+#endif
+	}
+}
+
+void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
 	}
 }

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -4,8 +4,8 @@ Copyright (c) 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, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -20,6 +20,8 @@ subject to the following restrictions:
 
 #include "btMultiBodyConstraint.h"
 
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
 class btMultiBodyPoint2Point : public btMultiBodyConstraint
 {
 protected:
@@ -28,7 +30,7 @@ protected:
 	btRigidBody*	m_rigidBodyB;
 	btVector3		m_pivotInA;
 	btVector3		m_pivotInB;
-	
+
 
 public:
 
@@ -37,6 +39,8 @@ public:
 
 	virtual ~btMultiBodyPoint2Point();
 
+	virtual void finalizeMultiDof();
+
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
@@ -54,7 +58,8 @@ public:
 		m_pivotInB = pivotInB;
 	}
 
-	
+	virtual void debugDraw(class btIDebugDraw* drawer);
+
 };
 
 #endif //BT_MULTIBODY_POINT2POINT_H

Source/ThirdParty/Bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 
 class btMultiBody;
+class btMultiBodyConstraint;
 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
@@ -28,16 +29,19 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
+	{}
 
 	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;
 	int				m_jacAindex;
-
 	int				m_deltaVelBindex;
+	int				m_jacBindex;
+
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal1;	
 	btVector3		m_relpos2CrossNormal;
 	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	int				m_jacBindex;
+	
 
 	btVector3		m_angularComponentA;
 	btVector3		m_angularComponentB;
@@ -70,6 +74,10 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 	btMultiBody* m_multiBodyB;
 	int			m_linkB;
 
+	//for writing back applied impulses
+	btMultiBodyConstraint*	m_orgConstraint;
+	int m_orgDofIndex;
+
 	enum		btSolverConstraintType
 	{
 		BT_SOLVER_CONTACT_1D = 0,

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

@@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
   /* declare variables - Z matrix, p and q vectors, etc */
   btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
   const btScalar *ell;
-  int lskip2,lskip3,i,j;
+  int lskip2,i,j;
+//  int lskip3;
   /* special handling for L and B because we're solving L1 *transpose* */
   L = L + (n-1)*(lskip1+1);
   B = B + n-1;
   lskip1 = -lskip1;
   /* compute lskip values */
   lskip2 = 2*lskip1;
-  lskip3 = 3*lskip1;
+  //lskip3 = 3*lskip1;
   /* compute all 4 x 1 blocks of X */
   for (i=0; i <= n-4; i+=4) {
     /* compute all 4 x 1 block of X, from rows i..i+4-1 */
@@ -1199,9 +1200,9 @@ struct btLCP
 	int *const m_findex, *const m_p, *const m_C;
 
 	btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-		btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
+		btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
 		btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-		bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows);
+		bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
 	int getNub() const { return m_nub; }
 	void transfer_i_to_C (int i);
 	void transfer_i_to_N (int i) { m_nN++; }			// because we can assume C and N span 1:i-1
@@ -1224,9 +1225,9 @@ struct btLCP
 
 
 btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-            btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d,
+            btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
             btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-            bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows):
+            bool *_state, int *_findex, int *p, int *c, btScalar **Arows):
   m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
 # ifdef BTROWPTRS
   m_A(Arows),
@@ -1234,8 +1235,8 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   m_A(_Adata),
 #endif
   m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
-  m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
-  m_state(_state), m_findex(_findex), m_p(_p), m_C(_C)
+  m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
+  m_state(_state), m_findex(_findex), m_p(p), m_C(c)
 {
   {
     btSetZero (m_x,m_n);

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

@@ -0,0 +1,371 @@
+/* Copyright (C) 2004-2013 MBSim Development Team
+
+Code was converted for the Bullet Continuous Collision Detection and Physics Library
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//The original version is here
+//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
+//This file is re-distributed under the ZLib license, with permission of the original author
+//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
+//STL/std::vector replaced by btAlignedObjectArray
+
+
+
+#include "btLemkeAlgorithm.h"
+
+#undef BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+using namespace std;
+#endif //BT_DEBUG_OSTREAM
+
+btScalar btMachEps()
+{
+	static bool calculated=false;
+	static btScalar machEps = btScalar(1.);
+	if (!calculated)
+	{
+		do {
+			machEps /= btScalar(2.0);
+			// If next epsilon yields 1, then break, because current
+			// epsilon is the machine epsilon.
+		}
+		while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
+//		printf( "\nCalculated Machine epsilon: %G\n", machEps );
+		calculated=true;
+	}
+	return machEps;
+}
+
+btScalar btEpsRoot() {
+	
+	static btScalar epsroot = 0.;
+	static bool alreadyCalculated = false;
+	
+	if (!alreadyCalculated) {
+		epsroot = btSqrt(btMachEps());
+		alreadyCalculated = true;
+	}
+	return epsroot;
+}
+
+	 
+
+  btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
+{
+  
+    
+    steps = 0;
+
+    int dim = m_q.size();
+#ifdef BT_DEBUG_OSTREAM
+    if(DEBUGLEVEL >= 1) {
+      cout << "Dimension = " << dim << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+	btVectorXu solutionVector(2 * dim);
+	solutionVector.setZero();
+	  
+	  //, INIT, 0.);
+
+	btMatrixXu ident(dim, dim);
+	ident.setIdentity();
+#ifdef BT_DEBUG_OSTREAM
+	cout << m_M << std::endl;
+#endif
+
+	btMatrixXu mNeg = m_M.negative();
+	  
+    btMatrixXu A(dim, 2 * dim + 2);
+	//
+	A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
+	A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
+	A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
+	A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
+
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //BT_DEBUG_OSTREAM
+
+
+ //   btVectorXu q_;
+ //   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
+
+    btAlignedObjectArray<int> basis;
+    //At first, all w-values are in the basis
+    for (int i = 0; i < dim; i++)
+      basis.push_back(i);
+
+	int pivotRowIndex = -1;
+	btScalar minValue = 1e30f;
+	bool greaterZero = true;
+	for (int i=0;i<dim;i++)
+	{
+		btScalar v =A(i,2*dim+1);
+		if (v<minValue)
+		{
+			minValue=v;
+			pivotRowIndex = i;
+		}
+		if (v<0)
+			greaterZero = false;
+	}
+	
+
+	
+  //  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
+    int z0Row = pivotRowIndex;           // remember the col of z0 for ending algorithm afterwards
+    int pivotColIndex = 2 * dim;         // first col is that of z0
+
+#ifdef BT_DEBUG_OSTREAM
+    if (DEBUGLEVEL >= 3)
+	{
+    //  cout << "A: " << A << endl;
+      cout << "pivotRowIndex " << pivotRowIndex << endl;
+      cout << "pivotColIndex " << pivotColIndex << endl;
+      cout << "Basis: ";
+      for (int i = 0; i < basis.size(); i++)
+        cout << basis[i] << " ";
+      cout << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+	if (!greaterZero)
+	{
+
+      if (maxloops == 0) {
+		  maxloops = 100;
+//        maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
+      }
+
+      /*start looping*/
+      for(steps = 0; steps < maxloops; steps++) {
+
+        GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
+#ifdef BT_DEBUG_OSTREAM
+        if (DEBUGLEVEL >= 3) {
+        //  cout << "A: " << A << endl;
+          cout << "pivotRowIndex " << pivotRowIndex << endl;
+          cout << "pivotColIndex " << pivotColIndex << endl;
+          cout << "Basis: ";
+          for (int i = 0; i < basis.size(); i++)
+            cout << basis[i] << " ";
+          cout << endl;
+        }
+#endif //BT_DEBUG_OSTREAM
+
+        int pivotColIndexOld = pivotColIndex;
+
+        /*find new column index */
+        if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
+          pivotColIndex = basis[pivotRowIndex] + dim;
+        else
+          //else do it the other way round and get in the corresponding w-value
+          pivotColIndex = basis[pivotRowIndex] - dim;
+
+        /*the column becomes part of the basis*/
+        basis[pivotRowIndex] = pivotColIndexOld;
+
+        pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
+
+        if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
+          GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
+          basis[pivotRowIndex] = pivotColIndex; //update basis
+          break;
+      }
+
+      }
+#ifdef BT_DEBUG_OSTREAM
+      if(DEBUGLEVEL >= 1) {
+        cout << "Number of loops: " << steps << endl;
+        cout << "Number of maximal loops: " << maxloops << endl;
+      }
+#endif //BT_DEBUG_OSTREAM
+
+      if(!validBasis(basis)) {
+        info = -1;
+#ifdef BT_DEBUG_OSTREAM
+        if(DEBUGLEVEL >= 1)
+          cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
+#endif //BT_DEBUG_OSTREAM
+
+        return solutionVector;
+      }
+
+    }
+#ifdef BT_DEBUG_OSTREAM
+    if (DEBUGLEVEL >= 2) {
+     // cout << "A: " << A << endl;
+      cout << "pivotRowIndex " << pivotRowIndex << endl;
+      cout << "pivotColIndex " << pivotColIndex << endl;
+    }
+#endif //BT_DEBUG_OSTREAM
+
+    for (int i = 0; i < basis.size(); i++)
+	{
+      solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
+	}
+
+    info = 0;
+
+    return solutionVector;
+  }
+
+  int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
+	  int RowIndex = 0;
+	  int dim = A.rows();
+	  btAlignedObjectArray<btVectorXu> Rows;
+	  for (int row = 0; row < dim; row++) 
+	  {
+
+		  btVectorXu vec(dim + 1);
+		  vec.setZero();//, INIT, 0.)
+		  Rows.push_back(vec);
+		  btScalar a = A(row, pivotColIndex);
+		  if (a > 0) {
+			  Rows[row][0] = A(row, 2 * dim + 1) / a;
+			  Rows[row][1] = A(row, 2 * dim) / a;
+			  for (int j = 2; j < dim + 1; j++)
+				  Rows[row][j] = A(row, j - 1) / a;
+
+#ifdef BT_DEBUG_OSTREAM
+		//		if (DEBUGLEVEL) {
+			//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
+				// }
+#endif 
+		  }
+	  }
+
+	  for (int i = 0; i < Rows.size(); i++) 
+	  {
+		  if (Rows[i].nrm2() > 0.) {
+
+			  int j = 0;
+			  for (; j < Rows.size(); j++) 
+			  {
+				  if(i != j)
+				  {
+					  if(Rows[j].nrm2() > 0.)
+					  {
+						  btVectorXu test(dim + 1);
+						  for (int ii=0;ii<dim+1;ii++)
+						  {
+							  test[ii] = Rows[j][ii] - Rows[i][ii];
+						  }
+
+						  //=Rows[j] - Rows[i]
+						  if (! LexicographicPositive(test))
+							  break;
+					  }
+				  }
+			  }
+
+			  if (j == Rows.size()) 
+			  {
+				  RowIndex += i;
+				  break;
+			  }
+		  }
+	  }
+
+	  return RowIndex;
+  }
+
+  bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
+{
+    int i = 0;
+  //  if (DEBUGLEVEL)
+    //  cout << "v " << v << endl;
+
+    while(i < v.size()-1 && fabs(v[i]) < btMachEps())
+      i++;
+    if (v[i] > 0)
+      return true;
+
+    return false;
+  }
+
+void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) 
+{
+
+	btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif
+
+    for (int i = 0; i < A.rows(); i++)
+	{
+      if (i != pivotRowIndex)
+	  {
+        for (int j = 0; j < A.cols(); j++)
+		{
+          if (j != pivotColumnIndex)
+		  {
+			  btScalar v = A(i, j);
+			  v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
+            A.setElem(i, j, v);
+		  }
+		}
+	  }
+	}
+
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //BT_DEBUG_OSTREAM
+    for (int i = 0; i < A.cols(); i++) 
+	{
+      A.mulElem(pivotRowIndex, i,-a);
+    }
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //#ifdef BT_DEBUG_OSTREAM
+
+    for (int i = 0; i < A.rows(); i++)
+	{
+      if (i != pivotRowIndex)
+	  {
+        A.setElem(i, pivotColumnIndex,0);
+	  }
+	}
+#ifdef BT_DEBUG_OSTREAM
+	cout << A << std::endl;
+#endif //#ifdef BT_DEBUG_OSTREAM
+  }
+
+  bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
+{
+    bool isGreater = true;
+    for (int i = 0; i < vector.size(); i++) {
+      if (vector[i] < 0) {
+        isGreater = false;
+        break;
+      }
+    }
+
+    return isGreater;
+  }
+
+  bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) 
+  {
+    bool isValid = true;
+    for (int i = 0; i < basis.size(); i++) {
+      if (basis[i] >= basis.size() * 2) { //then z0 is in the base
+        isValid = false;
+        break;
+      }
+    }
+
+    return isValid;
+  }
+
+

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h

@@ -0,0 +1,108 @@
+/* Copyright (C) 2004-2013 MBSim Development Team
+
+Code was converted for the Bullet Continuous Collision Detection and Physics Library
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//The original version is here
+//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
+//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
+//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
+//STL/std::vector replaced by btAlignedObjectArray
+
+
+
+#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
+#define BT_NUMERICS_LEMKE_ALGORITHM_H_
+
+#include "LinearMath/btMatrixX.h"
+
+
+#include <vector> //todo: replace by btAlignedObjectArray
+
+class btLemkeAlgorithm
+{
+public:
+ 
+
+  btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
+	  DEBUGLEVEL(DEBUGLEVEL_)
+  {
+	setSystem(M_, q_);
+  }
+
+  /* GETTER / SETTER */
+  /**
+   * \brief return info of solution process
+   */
+  int getInfo() {
+	return info;
+  }
+
+  /**
+   * \brief get the number of steps until the solution was found
+   */
+  int getSteps(void) {
+	return steps;
+  }
+
+
+
+  /**
+   * \brief set system with Matrix M and vector q
+   */
+  void setSystem(const btMatrixXu & M_, const btVectorXu & q_)
+	{
+		m_M = M_;
+		m_q = q_;
+  }
+  /***************************************************/
+
+  /**
+   * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)
+   */
+  btVectorXu solve(unsigned int maxloops = 0);
+
+  virtual ~btLemkeAlgorithm() {
+  }
+
+protected:
+  int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);
+  bool LexicographicPositive(const btVectorXu & v);
+  void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
+  bool greaterZero(const btVectorXu & vector);
+  bool validBasis(const btAlignedObjectArray<int>& basis);
+
+  btMatrixXu m_M;
+  btVectorXu m_q;
+
+  /**
+   * \brief number of steps until the Lemke algorithm found a solution
+   */
+  unsigned int steps;
+
+  /**
+   * \brief define level of debug output
+   */
+  int DEBUGLEVEL;
+
+  /**
+   * \brief did the algorithm find a solution
+   *
+   * -1 : not successful
+   *  0 : successful
+   */
+  int info;
+};
+
+
+#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

@@ -0,0 +1,350 @@
+/*
+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.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_LEMKE_SOLVER_H
+#define BT_LEMKE_SOLVER_H
+
+
+#include "btMLCPSolverInterface.h"
+#include "btLemkeAlgorithm.h"
+
+
+
+
+///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
+///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
+///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
+class btLemkeSolver : public btMLCPSolverInterface
+{
+protected:
+	
+public:
+
+	btScalar	m_maxValue;
+	int			m_debugLevel;
+	int			m_maxLoops;
+	bool		m_useLoHighBounds;
+	
+	
+
+	btLemkeSolver()
+		:m_maxValue(100000),
+		m_debugLevel(0),
+		m_maxLoops(1000),
+		m_useLoHighBounds(true)
+	{
+	}
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		
+		if (m_useLoHighBounds)
+		{
+
+		BT_PROFILE("btLemkeSolver::solveMLCP");
+		int n = A.rows();
+		if (0==n)
+			return true;
+		
+		bool fail = false;
+
+		btVectorXu solution(n);
+		btVectorXu q1;
+		q1.resize(n);
+		for (int row=0;row<n;row++)
+		{
+			q1[row] = -b[row];
+		}
+
+	//		cout << "A" << endl;
+	//		cout << A << endl;
+
+			/////////////////////////////////////
+
+			//slow matrix inversion, replace with LU decomposition
+			btMatrixXu A1;
+			btMatrixXu B(n,n);
+			{
+				BT_PROFILE("inverse(slow)");
+				A1.resize(A.rows(),A.cols());
+				for (int row=0;row<A.rows();row++)
+				{
+					for (int col=0;col<A.cols();col++)
+					{
+						A1.setElem(row,col,A(row,col));
+					}
+				}
+
+				btMatrixXu matrix;
+				matrix.resize(n,2*n);
+				for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						matrix.setElem(row,col,A1(row,col));
+					}
+				}
+
+
+				btScalar ratio,a;
+				int i,j,k;
+				for(i = 0; i < n; i++){
+				for(j = n; j < 2*n; j++){
+					if(i==(j-n))
+						matrix.setElem(i,j,1.0);
+					else
+						matrix.setElem(i,j,0.0);
+				}
+			}
+			for(i = 0; i < n; i++){
+				for(j = 0; j < n; j++){
+					if(i!=j)
+					{
+						btScalar v = matrix(i,i);
+						if (btFuzzyZero(v))
+						{
+							a = 0.000001f;
+						}
+						ratio = matrix(j,i)/matrix(i,i);
+						for(k = 0; k < 2*n; k++){
+							matrix.addElem(j,k,- ratio * matrix(i,k));
+						}
+					}
+				}
+			}
+			for(i = 0; i < n; i++){
+				a = matrix(i,i);
+				if (btFuzzyZero(a))
+				{
+					a = 0.000001f;
+				}
+				btScalar invA = 1.f/a;
+				for(j = 0; j < 2*n; j++){
+					matrix.mulElem(i,j,invA);
+				}
+			}
+
+	
+
+	
+
+			for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						B.setElem(row,col,matrix(row,n+col));
+					}
+				}
+			}
+
+		btMatrixXu b1(n,1);
+
+		btMatrixXu M(n*2,n*2);
+		for (int row=0;row<n;row++)
+		{
+			b1.setElem(row,0,-b[row]);
+			for (int col=0;col<n;col++)
+			{
+				btScalar v =B(row,col);
+				M.setElem(row,col,v);
+				M.setElem(n+row,n+col,v);
+				M.setElem(n+row,col,-v);
+				M.setElem(row,n+col,-v);
+
+			}
+		}
+
+		btMatrixXu Bb1 = B*b1;
+//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
+
+		btVectorXu qq;
+		qq.resize(n*2);
+		for (int row=0;row<n;row++)
+		{
+			qq[row] = -Bb1(row,0)-lo[row];
+			qq[n+row] = Bb1(row,0)+hi[row];
+		}
+
+		btVectorXu z1;
+
+		btMatrixXu y1;
+		y1.resize(n,1);
+		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
+		{
+			BT_PROFILE("lemke.solve");
+			lemke.setSystem(M,qq);
+			z1  = lemke.solve(m_maxLoops);
+		}
+		for (int row=0;row<n;row++)
+		{
+			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
+		}
+		btMatrixXu y1_b1(n,1);
+		for (int i=0;i<n;i++)
+		{
+			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
+		}
+
+		btMatrixXu x1;
+
+		x1 = B*(y1_b1);
+			
+		for (int row=0;row<n;row++)
+		{
+			solution[row] = x1(row,0);//n];
+		}
+
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<n;i++)
+		{
+			x[i] = solution[i];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				//printf("Lemke result is #NAN\n");
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			int m_errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			m_errorCountTimes++;
+		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<n;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+		return !fail;
+	} else
+		
+	{
+			int dimension = A.rows();
+		if (0==dimension)
+			return true;
+		
+//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+		btVectorXu q;
+		q.resize(dimension);
+		for (int row=0;row<dimension;row++)
+		{
+			q[row] = -b[row];
+		}
+		
+		btLemkeAlgorithm lemke(A,q,m_debugLevel);
+		
+		
+		lemke.setSystem(A,q);
+		
+		btVectorXu solution = lemke.solve(m_maxLoops);
+		
+		//check solution
+		
+		bool fail = false;
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<dimension;i++)
+		{
+			x[i] = solution[i+dimension];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			static int errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<dimension;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+
+
+		return !fail;
+	}
+	return true;
+
+	}
+
+};
+
+#endif //BT_LEMKE_SOLVER_H

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -19,9 +19,11 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
+
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
 :m_solver(solver),
-m_fallback(0)
+m_fallback(0),
+m_cfm(0.000001)//0.0000001
 {
 }
 
@@ -42,8 +44,8 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
 
 
-		int numBodies = m_tmpSolverBodyPool.size();
-		m_allConstraintArray.resize(0);
+	//	int numBodies = m_tmpSolverBodyPool.size();
+		m_allConstraintPtrArray.resize(0);
 		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
 		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
 	//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
@@ -51,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		int dindex = 0;
 		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
 		{
-			m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
 			m_limitDependencies[dindex++] = -1;
 		}
  
@@ -63,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		{
 			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
 			{
-				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
-				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
 				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
 				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
 				if (numFrictionPerContact==2)
 				{
-					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
 					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
 				}
 			}
@@ -78,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		{
 			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
 			{
-				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
 			}
 			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
 			{
-				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
 				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
 			}
 			
 		}
 
 
-		if (!m_allConstraintArray.size())
+		if (!m_allConstraintPtrArray.size())
 		{
 			m_A.resize(0,0);
 			m_b.resize(0);
@@ -154,26 +156,30 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 {
 	int numContactRows = interleaveContactAndFriction ? 3 : 1;
 
-	int numConstraintRows = m_allConstraintArray.size();
+	int numConstraintRows = m_allConstraintPtrArray.size();
 	int n = numConstraintRows;
 	{
 		BT_PROFILE("init b (rhs)");
 		m_b.resize(numConstraintRows);
 		m_bSplit.resize(numConstraintRows);
-		//m_b.setZero();
+		m_b.setZero();
+		m_bSplit.setZero();
 		for (int i=0;i<numConstraintRows ;i++)
 		{
-			if (m_allConstraintArray[i].m_jacDiagABInv)
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
 			{
-				m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
-				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i]=rhs/jacDiag;
+				m_bSplit[i] = rhsPenetration/jacDiag;
 			}
 
 		}
 	}
 
-	btScalar* w = 0;
-	int nub = 0;
+//	btScalar* w = 0;
+//	int nub = 0;
 
 	m_lo.resize(numConstraintRows);
 	m_hi.resize(numConstraintRows);
@@ -189,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 				m_hi[i] = BT_INFINITY;
 			} else
 			{
-				m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-				m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
 			}
 		}
 	}
 
 	//
-	int m=m_allConstraintArray.size();
+	int m=m_allConstraintPtrArray.size();
 
 	int numBodies = m_tmpSolverBodyPool.size();
 	btAlignedObjectArray<int> bodyJointNodeArray;
@@ -207,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	btAlignedObjectArray<btJointNode> jointNodeArray;
 	{
 		BT_PROFILE("jointNodeArray.reserve");
-		jointNodeArray.reserve(2*m_allConstraintArray.size());
+		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
 	}
 
 	static btMatrixXu J3;
@@ -229,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
-		ofs.resizeNoInitialize(m_allConstraintArray.size());
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
 	}				
 	{
 		BT_PROFILE("Compute J and JinvM");
@@ -237,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 		int numRows = 0;
 
-		for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
+		for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
 		{
 			ofs[c] = rowOffset;
-			int sbA = m_allConstraintArray[i].m_solverBodyIdA;
-			int sbB = m_allConstraintArray[i].m_solverBodyIdB;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
@@ -262,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 				}
 				for (int row=0;row<numRows;row++,cur++)
 				{
-					btVector3 normalInvMass =				m_allConstraintArray[i+row].m_contactNormal1 *		orgBodyA->getInvMass();
-					btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
+					btVector3 normalInvMass =				m_allConstraintPtrArray[i+row]->m_contactNormal1 *		orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
 
 					for (int r=0;r<3;r++)
 					{
-						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]);
-						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]);
+						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
+						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
 						JinvM3.setElem(cur,r,normalInvMass[r]);
 						JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
 					}
@@ -299,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 				for (int row=0;row<numRows;row++,cur++)
 				{
-					btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
-					btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
 
 					for (int r=0;r<3;r++)
 					{
-						J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]);
-						J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]);
+						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
+						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
 						JinvM3.setElem(cur,r,normalInvMassB[r]);
 						JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
 					}
@@ -343,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		int numRows = 0;
 		BT_PROFILE("Compute A");
-		for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
+		for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
 		{
 			int row__ = ofs[c];
-			int sbA = m_allConstraintArray[i].m_solverBodyIdA;
-			int sbB = m_allConstraintArray[i].m_solverBodyIdB;
-			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+		//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+		//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
 					
@@ -365,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					{
 								 
 						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintArray[cr0].m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
 						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
 						m_A.multiplyAdd2_p8r ( JinvMrow, 
 						Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther,  row__,ofs[j0]);
@@ -384,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					if (j1<c)
 					{
 						int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintArray[cj1].m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
 						m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 
 						Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
 					}
@@ -398,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			// compute diagonal blocks of m_A
 
 			int  row__ = 0;
-			int numJointRows = m_allConstraintArray.size();
+			int numJointRows = m_allConstraintPtrArray.size();
 
 			int jj=0;
 			for (;row__<numJointRows;)
 			{
 
-				int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
-				int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
-				btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 
@@ -425,14 +431,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		}
 	}
 
-	///todo: use proper cfm values from the constraints (getInfo2)
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			float cfm = 0.00001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 				   
@@ -449,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
 		{
-			for (int i=0;i<m_allConstraintArray.size();i++)
+			for (int i=0;i<m_allConstraintPtrArray.size();i++)
 			{
-				const btSolverConstraint& c = m_allConstraintArray[i];
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 				m_x[i]=c.m_appliedImpulse;
 				m_xSplit[i] = c.m_appliedPushImpulse;
 			}
@@ -467,19 +471,22 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 {
 	int numBodies = this->m_tmpSolverBodyPool.size();
-	int numConstraintRows = m_allConstraintArray.size();
+	int numConstraintRows = m_allConstraintPtrArray.size();
 
 	m_b.resize(numConstraintRows);
 	if (infoGlobal.m_splitImpulse)
 		m_bSplit.resize(numConstraintRows);
  
+	m_bSplit.setZero();
+	m_b.setZero();
+
 	for (int i=0;i<numConstraintRows ;i++)
 	{
-		if (m_allConstraintArray[i].m_jacDiagABInv)
+		if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
 		{
-			m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+			m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (infoGlobal.m_splitImpulse)
-				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
 		}
 	}
  
@@ -510,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	for (int i=0;i<numConstraintRows;i++)
 	{
 
-		m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-		m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+		m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+		m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
  
-		int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
-		int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
+		int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+		int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 		if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
 		{
-			setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]);
-			setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]);
-			setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]);
-			setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]);
-			setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]);
-			setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]);
+			setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]);
+			setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]);
+			setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]);
+			setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
+			setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
+			setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
 		}
 		if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
 		{
-			setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]);
-			setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]);
-			setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]);
-			setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]);
-			setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]);
-			setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]);
+			setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]);
+			setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]);
+			setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]);
+			setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
+			setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
+			setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
 		}
 	}
  
@@ -554,12 +561,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 	if (1)
 	{
-		///todo: use proper cfm values from the constraints (getInfo2)
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			float cfm = 0.0001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 
@@ -568,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		m_xSplit.resize(numConstraintRows);
 //	m_x.setZero();
 
-	for (int i=0;i<m_allConstraintArray.size();i++)
+	for (int i=0;i<m_allConstraintPtrArray.size();i++)
 	{
-		const btSolverConstraint& c = m_allConstraintArray[i];
+		const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 		m_x[i]=c.m_appliedImpulse;
 		if (infoGlobal.m_splitImpulse)
 			m_xSplit[i] = c.m_appliedPushImpulse;
@@ -592,35 +597,44 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	if (result)
 	{
 		BT_PROFILE("process MLCP results");
-		for (int i=0;i<m_allConstraintArray.size();i++)
+		for (int i=0;i<m_allConstraintPtrArray.size();i++)
 		{
 			{
-				btSolverConstraint& c = m_allConstraintArray[i];
+				btSolverConstraint& c = *m_allConstraintPtrArray[i];
 				int sbA = c.m_solverBodyIdA;
 				int sbB = c.m_solverBodyIdB;
-				btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+				//btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
  
-				solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
-				solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
+				{
+					btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
+					c.m_appliedImpulse = m_x[i];
+					solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+					solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+				}
+
 				if (infoGlobal.m_splitImpulse)
 				{
-					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
-					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
+					btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 					c.m_appliedPushImpulse = m_xSplit[i];
 				}
-				c.m_appliedImpulse = m_x[i];
+				
 			}
 		}
 	}
 	else
 	{
+	//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
 		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	}
 
 	return 0.f;
-}
+}
+
+

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -39,12 +39,15 @@ protected:
 	btVectorXu m_xSplit2;
 
 	btAlignedObjectArray<int> m_limitDependencies;
-	btConstraintArray m_allConstraintArray;
+	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
+	btScalar m_cfm;
 
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+
 	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
 	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
 
@@ -70,6 +73,15 @@ public:
 		m_fallback = num;
 	}
 
+	btScalar	getCfm() const
+	{
+		return m_cfm;
+	}
+	void setCfm(btScalar cfm)
+	{
+		m_cfm = cfm;
+	}
+
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_MLCP_SOLVER;

Source/ThirdParty/Bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -20,11 +20,17 @@ subject to the following restrictions:
 
 #include "btMLCPSolverInterface.h"
 
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
 {
 public:
 	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
+		if (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
 		//A is a m-n matrix, m rows, n columns
 		btAssert(A.rows() == b.rows());
 
@@ -56,7 +62,7 @@ public:
 				}
 
 				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / A(i,i);
+				x [i] = (b [i] - delta) / aDiag;
 				float s = 1.f;
 
 				if (limitDependency[i]>=0)

Source/ThirdParty/Bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	int i=0;
 	for (i=0;i<m_wheelInfo.size();i++)
 	{
-		btScalar depth; 
-		depth = rayCast( m_wheelInfo[i]);
+		//btScalar depth; 
+		//depth = 
+		rayCast( m_wheelInfo[i]);
 	}
 
 	updateSuspension(step);