Merge pull request #2265 from akien-mga/dos2unix

Convert DOS (CRLF) source files to Unix (LF) line endings

29 files changed, 9147 insertions, 9147 deletions

diff --git a/README.md b/README.md
index 276ca597c..6efe9e5e4 100644
--- a/README.md
+++ b/README.md
@@ -1,118 +1,118 @@
-[![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 Physics SDK

-

-This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.

-

-New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).

-

-The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.

-The steps towards a new API is in a nutshell:

-

-1. The old Bullet2 demos are being merged into the examples/ExampleBrowser

-2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h

-3. Python bindings in pybullet are on top of this C-API, see examples/pybullet

-4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available

-5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl

-

-You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk

-

-## Requirements for Bullet 2

-

-A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,

-but should likely work on any platform with C++ compiler. 

-Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.

-

-## Contributors and Coding Style information

-

-https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit

-

-## Requirements for experimental OpenCL GPGPU support

-

-The entire collision detection and rigid body dynamics can be executed on the GPU.

-

-A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.

-We succesfully tested the software under Windows, Linux and Mac OSX.

-The software currently doesn't work on OpenCL CPU devices. It might run

-on a laptop GPU but performance will not likely be very good. Note that

-often an OpenCL drivers fails to compile a kernel. Some unit tests exist to

-track down the issue, but more work is required to cover all OpenCL kernels.

-

-## License

-

-All source code files are licensed under the permissive zlib license

-(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.

-

-## Build instructions for Bullet using premake. You can also use cmake instead.

-

-**Windows**

-

-Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln

-When asked, convert the projects to a newer version of Visual Studio.

-If you installed Python in the C:\ root directory, the batch file should find it automatically.

-Otherwise, edit this batch file to choose where Python include/lib directories are located.

-

-**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**

-

-Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.

-You can connect from Python pybullet to the sandbox using:

-

-```

-import pybullet as p

-p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234)

-```

-

-**Linux and Mac OSX gnu make**

-

-Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)

-

-In a terminal type:

-

-	./build_cmake_pybullet_double.sh

-

-This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.

-The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.

-

-You can also build Bullet using premake. There are premake executables in the build3 folder.

-Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines

-Using premake:

-```

-	cd build3

-	./premake4_linux --double gmake

-	./premake4_linux64 --double gmake

-	./premake4_osx --double --enable_pybullet gmake

-```

-Then

-```

-	cd gmake

-	make

-```

-

-Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.

-

-**Mac OSX Xcode**

-	

-Click on build3/xcode4.command or in a terminal window execute

-	

-	./premake_osx xcode4

-

-## Usage

-

-The App_ExampleBrowser executables will be located in the bin folder.

-You can just run it though a terminal/command prompt, or by clicking it.

-

-

-```

-[--start_demo_name="Demo Name"]     Start with a selected demo  

-[--mp4=moviename.mp4]               Create a mp4 movie of the window, requires ffmpeg installed

-[--mouse_move_multiplier=0.400000]  Set the mouse move sensitivity

-[--mouse_wheel_multiplier=0.01]     Set the mouse wheel sensitivity

-[--background_color_red= 0.9]       Set the red component for background color. Same for green and blue

-[--fixed_timestep= 0.0]             Use either a real-time delta time (0.0) or a fixed step size (0.016666)

-```

-

-You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.

-Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.

-

-Check out the docs folder and the Bullet physics forums for further information.

+[![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 Physics SDK
+
+This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
+
+New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
+
+The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
+The steps towards a new API is in a nutshell:
+
+1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
+2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
+3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
+4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
+5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
+
+You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
+
+## Requirements for Bullet 2
+
+A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android,
+but should likely work on any platform with C++ compiler. 
+Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too.
+
+## Contributors and Coding Style information
+
+https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
+
+## Requirements for experimental OpenCL GPGPU support
+
+The entire collision detection and rigid body dynamics can be executed on the GPU.
+
+A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
+We succesfully tested the software under Windows, Linux and Mac OSX.
+The software currently doesn't work on OpenCL CPU devices. It might run
+on a laptop GPU but performance will not likely be very good. Note that
+often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
+track down the issue, but more work is required to cover all OpenCL kernels.
+
+## License
+
+All source code files are licensed under the permissive zlib license
+(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file.
+
+## Build instructions for Bullet using premake. You can also use cmake instead.
+
+**Windows**
+
+Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
+When asked, convert the projects to a newer version of Visual Studio.
+If you installed Python in the C:\ root directory, the batch file should find it automatically.
+Otherwise, edit this batch file to choose where Python include/lib directories are located.
+
+**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
+
+Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
+You can connect from Python pybullet to the sandbox using:
+
+```
+import pybullet as p
+p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234)
+```
+
+**Linux and Mac OSX gnu make**
+
+Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
+
+In a terminal type:
+
+	./build_cmake_pybullet_double.sh
+
+This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.
+The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.
+
+You can also build Bullet using premake. There are premake executables in the build3 folder.
+Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
+Using premake:
+```
+	cd build3
+	./premake4_linux --double gmake
+	./premake4_linux64 --double gmake
+	./premake4_osx --double --enable_pybullet gmake
+```
+Then
+```
+	cd gmake
+	make
+```
+
+Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.
+
+**Mac OSX Xcode**
+	
+Click on build3/xcode4.command or in a terminal window execute
+	
+	./premake_osx xcode4
+
+## Usage
+
+The App_ExampleBrowser executables will be located in the bin folder.
+You can just run it though a terminal/command prompt, or by clicking it.
+
+
+```
+[--start_demo_name="Demo Name"]     Start with a selected demo  
+[--mp4=moviename.mp4]               Create a mp4 movie of the window, requires ffmpeg installed
+[--mouse_move_multiplier=0.400000]  Set the mouse move sensitivity
+[--mouse_wheel_multiplier=0.01]     Set the mouse wheel sensitivity
+[--background_color_red= 0.9]       Set the red component for background color. Same for green and blue
+[--fixed_timestep= 0.0]             Use either a real-time delta time (0.0) or a fixed step size (0.016666)
+```
+
+You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
+Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
+
+Check out the docs folder and the Bullet physics forums for further information.
diff --git a/build3/Android/jni/Android.mk b/build3/Android/jni/Android.mk
index d884e7c96..157e3995a 100644
--- a/build3/Android/jni/Android.mk
+++ b/build3/Android/jni/Android.mk
@@ -1,49 +1,49 @@
-LOCAL_PATH := ../../..

-

-include $(CLEAR_VARS)

-

-LOCAL_CFLAGS := $(LOCAL_C_INCLUDES:%=-I%) -DUSE_PTHREADS -mfpu=neon -mfloat-abi=softfp -pthread -DSCE_PFX_USE_SIMD_VECTORMATH

-

-# apply these flags if needed 

-# -ffast-math -funsafe-math-optimizations

-

-# apply this to disable optimization

-# TARGET_CFLAGS := $(TARGET_CFLAGS) -O0

-

-# apply these 2 to turn on assembly output (*.c/*.cpp to *.s file)

-#compile-cpp-source = $(eval $(call ev-compile-cpp-source,$1,$(1:%$(LOCAL_CPP_EXTENSION)=%.s)))

-#TARGET_CFLAGS := $(TARGET_CFLAGS) -S

-

-# Enable or disable NEON. Don't forget to apply, or not apply, -mfpu=neon and -mfloat-abi=softfp

-# flags in addition, e.g., if this is true both of those need to be included in LOCAL_CFLAGS

-# to avoid the possibility that ndk-build will "forget" to add them on some files

-LOCAL_ARM_NEON := true

-

-TARGET_CFLAGS := $(filter-out -ffpu=vfp,$(TARGET_CFLAGS))

-

-# setup to build static library

-LOCAL_MODULE := libBullet

-

-LOCAL_C_INCLUDES :=  $(LOCAL_PATH)/src

-

-#find all the file recursively under jni/

-FILE_LIST := $(wildcard \

-		$(LOCAL_PATH)/src/LinearMath/*.cpp \

-		$(LOCAL_PATH)/src/Bullet3Common/*.cpp \

-		$(LOCAL_PATH)/src/BulletCollision/BroadphaseCollision/*.cpp \

-		$(LOCAL_PATH)/src/BulletCollision/CollisionDispatch/*.cpp \

-		$(LOCAL_PATH)/src/BulletCollision/CollisionShapes/*.cpp \

-		$(LOCAL_PATH)/src/BulletCollision/NarrowPhaseCollision/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/ConstraintSolver/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/Dynamics/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/Featherstone/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/MLCPSolvers/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/Vehicle/*.cpp \

-		$(LOCAL_PATH)/src/BulletDynamics/Character/*.cpp \

-		$(LOCAL_PATH)/src/BulletSoftBody/*.cpp \

-		$(LOCAL_PATH)/src/BulletInverseDynamics/*.cpp \

-		$(LOCAL_PATH)/src/BulletInverseDynamics/details/*.cpp \

-		)

-LOCAL_SRC_FILES := $(FILE_LIST:$(LOCAL_PATH)/%=%)

-

-include $(BUILD_STATIC_LIBRARY)

+LOCAL_PATH := ../../..
+
+include $(CLEAR_VARS)
+
+LOCAL_CFLAGS := $(LOCAL_C_INCLUDES:%=-I%) -DUSE_PTHREADS -mfpu=neon -mfloat-abi=softfp -pthread -DSCE_PFX_USE_SIMD_VECTORMATH
+
+# apply these flags if needed 
+# -ffast-math -funsafe-math-optimizations
+
+# apply this to disable optimization
+# TARGET_CFLAGS := $(TARGET_CFLAGS) -O0
+
+# apply these 2 to turn on assembly output (*.c/*.cpp to *.s file)
+#compile-cpp-source = $(eval $(call ev-compile-cpp-source,$1,$(1:%$(LOCAL_CPP_EXTENSION)=%.s)))
+#TARGET_CFLAGS := $(TARGET_CFLAGS) -S
+
+# Enable or disable NEON. Don't forget to apply, or not apply, -mfpu=neon and -mfloat-abi=softfp
+# flags in addition, e.g., if this is true both of those need to be included in LOCAL_CFLAGS
+# to avoid the possibility that ndk-build will "forget" to add them on some files
+LOCAL_ARM_NEON := true
+
+TARGET_CFLAGS := $(filter-out -ffpu=vfp,$(TARGET_CFLAGS))
+
+# setup to build static library
+LOCAL_MODULE := libBullet
+
+LOCAL_C_INCLUDES :=  $(LOCAL_PATH)/src
+
+#find all the file recursively under jni/
+FILE_LIST := $(wildcard \
+		$(LOCAL_PATH)/src/LinearMath/*.cpp \
+		$(LOCAL_PATH)/src/Bullet3Common/*.cpp \
+		$(LOCAL_PATH)/src/BulletCollision/BroadphaseCollision/*.cpp \
+		$(LOCAL_PATH)/src/BulletCollision/CollisionDispatch/*.cpp \
+		$(LOCAL_PATH)/src/BulletCollision/CollisionShapes/*.cpp \
+		$(LOCAL_PATH)/src/BulletCollision/NarrowPhaseCollision/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/ConstraintSolver/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/Dynamics/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/Featherstone/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/MLCPSolvers/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/Vehicle/*.cpp \
+		$(LOCAL_PATH)/src/BulletDynamics/Character/*.cpp \
+		$(LOCAL_PATH)/src/BulletSoftBody/*.cpp \
+		$(LOCAL_PATH)/src/BulletInverseDynamics/*.cpp \
+		$(LOCAL_PATH)/src/BulletInverseDynamics/details/*.cpp \
+		)
+LOCAL_SRC_FILES := $(FILE_LIST:$(LOCAL_PATH)/%=%)
+
+include $(BUILD_STATIC_LIBRARY)
diff --git a/build3/Android/jni/Application.mk b/build3/Android/jni/Application.mk
index dca8463c5..05057fcf2 100644
--- a/build3/Android/jni/Application.mk
+++ b/build3/Android/jni/Application.mk
@@ -1,7 +1,7 @@
-APP_MODULES := libBullet

-APP_ABI := armeabi-v7a

-APP_OPTIM := release

-

-#We only need STL for placement new (#include <new>) 

-#We don't use STL in Bullet

-APP_STL                 := stlport_static

+APP_MODULES := libBullet
+APP_ABI := armeabi-v7a
+APP_OPTIM := release
+
+#We only need STL for placement new (#include <new>) 
+#We don't use STL in Bullet
+APP_STL                 := stlport_static
diff --git a/examples/ExtendedTutorials/InclinedPlane.h b/examples/ExtendedTutorials/InclinedPlane.h
index fc61fe174..0a7c2d371 100644
--- a/examples/ExtendedTutorials/InclinedPlane.h
+++ b/examples/ExtendedTutorials/InclinedPlane.h
@@ -1,21 +1,21 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2015 Google 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 ET_INCLINED_PLANE_EXAMPLE_H

-#define ET_INCLINED_PLANE_EXAMPLE_H

-

-class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);

-

-#endif  //ET_INCLINED_PLANE_EXAMPLE_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google 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 ET_INCLINED_PLANE_EXAMPLE_H
+#define ET_INCLINED_PLANE_EXAMPLE_H
+
+class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //ET_INCLINED_PLANE_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/MultiPendulum.h b/examples/ExtendedTutorials/MultiPendulum.h
index 75e67fe86..d65a295f3 100644
--- a/examples/ExtendedTutorials/MultiPendulum.h
+++ b/examples/ExtendedTutorials/MultiPendulum.h
@@ -1,21 +1,21 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2015 Google 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 ET_MULTI_PENDULUM_EXAMPLE_H

-#define ET_MULTI_PENDULUM_EXAMPLE_H

-

-class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);

-

-#endif  //ET_MULTI_PENDULUM_EXAMPLE_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google 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 ET_MULTI_PENDULUM_EXAMPLE_H
+#define ET_MULTI_PENDULUM_EXAMPLE_H
+
+class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //ET_MULTI_PENDULUM_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/NewtonsCradle.h b/examples/ExtendedTutorials/NewtonsCradle.h
index 16099834d..439003d38 100644
--- a/examples/ExtendedTutorials/NewtonsCradle.h
+++ b/examples/ExtendedTutorials/NewtonsCradle.h
@@ -1,21 +1,21 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2015 Google 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 ET_NEWTONS_CRADLE_EXAMPLE_H

-#define ET_NEWTONS_CRADLE_EXAMPLE_H

-

-class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);

-

-#endif  //ET_NEWTONS_CRADLE_EXAMPLE_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google 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 ET_NEWTONS_CRADLE_EXAMPLE_H
+#define ET_NEWTONS_CRADLE_EXAMPLE_H
+
+class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //ET_NEWTONS_CRADLE_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/NewtonsRopeCradle.h b/examples/ExtendedTutorials/NewtonsRopeCradle.h
index d2907db70..e7ddcf2db 100644
--- a/examples/ExtendedTutorials/NewtonsRopeCradle.h
+++ b/examples/ExtendedTutorials/NewtonsRopeCradle.h
@@ -1,21 +1,21 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2015 Google 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 ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H

-#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H

-

-class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);

-

-#endif  //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google 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 ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
+#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
+
+class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py
index 2bfc45093..df2d37d3c 100644
--- a/examples/pybullet/examples/createObstacleCourse.py
+++ b/examples/pybullet/examples/createObstacleCourse.py
@@ -1,128 +1,128 @@
-import pybullet as p

-import time

-import math

-

-p.connect(p.GUI)

-#don't create a ground plane, to allow for gaps etc

-p.resetSimulation()

-#p.createCollisionShape(p.GEOM_PLANE)

-#p.createMultiBody(0,0)

-#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);

-p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])

-

-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)

-

-sphereRadius = 0.05

-colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)

-

-#a few different ways to create a mesh:

-

-#convex mesh from obj

-stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")

-

-boxHalfLength = 0.5

-boxHalfWidth = 2.5

-boxHalfHeight = 0.1

-segmentLength = 5

-

-colBoxId = p.createCollisionShape(p.GEOM_BOX,

-                                  halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])

-

-mass = 1

-visualShapeId = -1

-

-segmentStart = 0

-

-for i in range(segmentLength):

-  p.createMultiBody(baseMass=0,

-                    baseCollisionShapeIndex=colBoxId,

-                    basePosition=[segmentStart, 0, -0.1])

-  segmentStart = segmentStart - 1

-

-for i in range(segmentLength):

-  height = 0

-  if (i % 2):

-    height = 1

-  p.createMultiBody(baseMass=0,

-                    baseCollisionShapeIndex=colBoxId,

-                    basePosition=[segmentStart, 0, -0.1 + height])

-  segmentStart = segmentStart - 1

-

-baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])

-

-for i in range(segmentLength):

-  p.createMultiBody(baseMass=0,

-                    baseCollisionShapeIndex=colBoxId,

-                    basePosition=[segmentStart, 0, -0.1])

-  segmentStart = segmentStart - 1

-  if (i % 2):

-    p.createMultiBody(baseMass=0,

-                      baseCollisionShapeIndex=colBoxId,

-                      basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],

-                      baseOrientation=baseOrientation)

-

-for i in range(segmentLength):

-  p.createMultiBody(baseMass=0,

-                    baseCollisionShapeIndex=colBoxId,

-                    basePosition=[segmentStart, 0, -0.1])

-  width = 4

-  for j in range(width):

-    p.createMultiBody(baseMass=0,

-                      baseCollisionShapeIndex=stoneId,

-                      basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])

-  segmentStart = segmentStart - 1

-

-link_Masses = [1]

-linkCollisionShapeIndices = [colBoxId]

-linkVisualShapeIndices = [-1]

-linkPositions = [[0, 0, 0]]

-linkOrientations = [[0, 0, 0, 1]]

-linkInertialFramePositions = [[0, 0, 0]]

-linkInertialFrameOrientations = [[0, 0, 0, 1]]

-indices = [0]

-jointTypes = [p.JOINT_REVOLUTE]

-axis = [[1, 0, 0]]

-

-baseOrientation = [0, 0, 0, 1]

-for i in range(segmentLength):

-  boxId = p.createMultiBody(0,

-                            colSphereId,

-                            -1, [segmentStart, 0, -0.1],

-                            baseOrientation,

-                            linkMasses=link_Masses,

-                            linkCollisionShapeIndices=linkCollisionShapeIndices,

-                            linkVisualShapeIndices=linkVisualShapeIndices,

-                            linkPositions=linkPositions,

-                            linkOrientations=linkOrientations,

-                            linkInertialFramePositions=linkInertialFramePositions,

-                            linkInertialFrameOrientations=linkInertialFrameOrientations,

-                            linkParentIndices=indices,

-                            linkJointTypes=jointTypes,

-                            linkJointAxis=axis)

-  p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)

-  print(p.getNumJoints(boxId))

-  for joint in range(p.getNumJoints(boxId)):

-    targetVelocity = 10

-    if (i % 2):

-      targetVelocity = -10

-    p.setJointMotorControl2(boxId,

-                            joint,

-                            p.VELOCITY_CONTROL,

-                            targetVelocity=targetVelocity,

-                            force=100)

-  segmentStart = segmentStart - 1.1

-

-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

-while (1):

-  camData = p.getDebugVisualizerCamera()

-  viewMat = camData[2]

-  projMat = camData[3]

-  p.getCameraImage(256,

-                   256,

-                   viewMatrix=viewMat,

-                   projectionMatrix=projMat,

-                   renderer=p.ER_BULLET_HARDWARE_OPENGL)

-  keys = p.getKeyboardEvents()

-  p.stepSimulation()

-  #print(keys)

-  time.sleep(0.01)

+import pybullet as p
+import time
+import math
+
+p.connect(p.GUI)
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+
+#a few different ways to create a mesh:
+
+#convex mesh from obj
+stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+
+for i in range(segmentLength):
+  height = 0
+  if (i % 2):
+    height = 1
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1 + height])
+  segmentStart = segmentStart - 1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  segmentStart = segmentStart - 1
+  if (i % 2):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=colBoxId,
+                      basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
+                      baseOrientation=baseOrientation)
+
+for i in range(segmentLength):
+  p.createMultiBody(baseMass=0,
+                    baseCollisionShapeIndex=colBoxId,
+                    basePosition=[segmentStart, 0, -0.1])
+  width = 4
+  for j in range(width):
+    p.createMultiBody(baseMass=0,
+                      baseCollisionShapeIndex=stoneId,
+                      basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
+  segmentStart = segmentStart - 1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[1, 0, 0]]
+
+baseOrientation = [0, 0, 0, 1]
+for i in range(segmentLength):
+  boxId = p.createMultiBody(0,
+                            colSphereId,
+                            -1, [segmentStart, 0, -0.1],
+                            baseOrientation,
+                            linkMasses=link_Masses,
+                            linkCollisionShapeIndices=linkCollisionShapeIndices,
+                            linkVisualShapeIndices=linkVisualShapeIndices,
+                            linkPositions=linkPositions,
+                            linkOrientations=linkOrientations,
+                            linkInertialFramePositions=linkInertialFramePositions,
+                            linkInertialFrameOrientations=linkInertialFrameOrientations,
+                            linkParentIndices=indices,
+                            linkJointTypes=jointTypes,
+                            linkJointAxis=axis)
+  p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
+  print(p.getNumJoints(boxId))
+  for joint in range(p.getNumJoints(boxId)):
+    targetVelocity = 10
+    if (i % 2):
+      targetVelocity = -10
+    p.setJointMotorControl2(boxId,
+                            joint,
+                            p.VELOCITY_CONTROL,
+                            targetVelocity=targetVelocity,
+                            force=100)
+  segmentStart = segmentStart - 1.1
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+while (1):
+  camData = p.getDebugVisualizerCamera()
+  viewMat = camData[2]
+  projMat = camData[3]
+  p.getCameraImage(256,
+                   256,
+                   viewMatrix=viewMat,
+                   projectionMatrix=projMat,
+                   renderer=p.ER_BULLET_HARDWARE_OPENGL)
+  keys = p.getKeyboardEvents()
+  p.stepSimulation()
+  #print(keys)
+  time.sleep(0.01)
diff --git a/examples/pybullet/examples/loadingBench.py b/examples/pybullet/examples/loadingBench.py
index 38a12e12d..fcd886df5 100644
--- a/examples/pybullet/examples/loadingBench.py
+++ b/examples/pybullet/examples/loadingBench.py
@@ -1,26 +1,26 @@
-import pybullet as p

-import time

-p.connect(p.GUI)

-

-p.resetSimulation()

-timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")

-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)

-print("load plane.urdf")

-p.loadURDF("plane.urdf")

-print("load r2d2.urdf")

-

-p.loadURDF("r2d2.urdf", 0, 0, 1)

-print("load kitchen/1.sdf")

-p.loadSDF("kitchens/1.sdf")

-print("load 100 times plate.urdf")

-for i in range(100):

-  p.loadURDF("dinnerware/plate.urdf", 0, i, 1)

-

-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

-

-p.stopStateLogging(timinglog)

-print("stopped state logging")

-p.getCameraImage(320, 200)

-

-while (1):

-  p.stepSimulation()

+import pybullet as p
+import time
+p.connect(p.GUI)
+
+p.resetSimulation()
+timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+print("load plane.urdf")
+p.loadURDF("plane.urdf")
+print("load r2d2.urdf")
+
+p.loadURDF("r2d2.urdf", 0, 0, 1)
+print("load kitchen/1.sdf")
+p.loadSDF("kitchens/1.sdf")
+print("load 100 times plate.urdf")
+for i in range(100):
+  p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
+
+p.stopStateLogging(timinglog)
+print("stopped state logging")
+p.getCameraImage(320, 200)
+
+while (1):
+  p.stepSimulation()
diff --git a/examples/pybullet/examples/rendertest.py b/examples/pybullet/examples/rendertest.py
index aa53b61a1..6e3d14b89 100644
--- a/examples/pybullet/examples/rendertest.py
+++ b/examples/pybullet/examples/rendertest.py
@@ -1,150 +1,150 @@
-#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled

-#otherwise use testrender.py (slower but compatible without numpy)

-#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)

-

-import os

-import sys

-import time

-import itertools

-import subprocess

-import numpy as np

-import pybullet

-from multiprocessing import Process

-

-camTargetPos = [0, 0, 0]

-cameraUp = [0, 0, 1]

-cameraPos = [1, 1, 1]

-

-pitch = -10.0

-roll = 0

-upAxisIndex = 2

-camDistance = 4

-pixelWidth = 84  # 320

-pixelHeight = 84  # 200

-nearPlane = 0.01

-farPlane = 100

-fov = 60

-

-import matplotlib.pyplot as plt

-

-

-class BulletSim():

-

-  def __init__(self, connection_mode, *argv):

-    self.connection_mode = connection_mode

-    self.argv = argv

-

-  def __enter__(self):

-    print("connecting")

-    optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)

-    optionstring += ' --window_backend=2 --render_device=0'

-

-    print(self.connection_mode, optionstring, *self.argv)

-    cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)

-    if cid < 0:

-      raise ValueError

-    print("connected")

-    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)

-    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)

-    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)

-    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)

-

-    pybullet.resetSimulation()

-    pybullet.loadURDF("plane.urdf", [0, 0, -1])

-    pybullet.loadURDF("r2d2.urdf")

-    pybullet.loadURDF("duck_vhacd.urdf")

-    pybullet.setGravity(0, 0, -10)

-

-  def __exit__(self, *_, **__):

-    pybullet.disconnect()

-

-

-def test(num_runs=300, shadow=1, log=True, plot=False):

-  if log:

-    logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")

-

-  if plot:

-    plt.ion()

-

-    img = np.random.rand(200, 320)

-    #img = [tandard_normal((50,100))

-    image = plt.imshow(img, interpolation='none', animated=True, label="blah")

-    ax = plt.gca()

-

-  times = np.zeros(num_runs)

-  yaw_gen = itertools.cycle(range(0, 360, 10))

-  for i, yaw in zip(range(num_runs), yaw_gen):

-    pybullet.stepSimulation()

-    start = time.time()

-    viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,

-                                                            roll, upAxisIndex)

-    aspect = pixelWidth / pixelHeight

-    projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)

-    img_arr = pybullet.getCameraImage(pixelWidth,

-                                      pixelHeight,

-                                      viewMatrix,

-                                      projectionMatrix,

-                                      shadow=shadow,

-                                      lightDirection=[1, 1, 1],

-                                      renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)

-    #renderer=pybullet.ER_TINY_RENDERER)

-    stop = time.time()

-    duration = (stop - start)

-    if (duration):

-      fps = 1. / duration

-      #print("fps=",fps)

-    else:

-      fps = 0

-      #print("fps=",fps)

-    #print("duraction=",duration)

-    #print("fps=",fps)

-    times[i] = fps

-

-    if plot:

-      rgb = img_arr[2]

-      image.set_data(rgb)  #np_img_arr)

-      ax.plot([0])

-      #plt.draw()

-      #plt.show()

-      plt.pause(0.01)

-

-  mean_time = float(np.mean(times))

-  print("mean: {0} for {1} runs".format(mean_time, num_runs))

-  print("")

-  if log:

-    pybullet.stopStateLogging(logId)

-  return mean_time

-

-

-if __name__ == "__main__":

-

-  res = []

-

-  with BulletSim(pybullet.DIRECT):

-    print("\nTesting DIRECT")

-    mean_time = test(log=False, plot=True)

-    res.append(("tiny", mean_time))

-

-  with BulletSim(pybullet.DIRECT):

-    plugin_fn = os.path.join(

-        pybullet.__file__.split("bullet3")[0],

-        "bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")

-    plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")

-    if plugin < 0:

-      print("\nPlugin Failed to load!\n")

-      sys.exit()

-

-    print("\nTesting DIRECT+OpenGL")

-    mean_time = test(log=True)

-    res.append(("plugin", mean_time))

-

-  with BulletSim(pybullet.GUI):

-    print("\nTesting GUI")

-    mean_time = test(log=False)

-    res.append(("egl", mean_time))

-

-  print()

-  print("rendertest.py")

-  print("back nenv fps fps_tot")

-  for r in res:

-    print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))

+#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
+#otherwise use testrender.py (slower but compatible without numpy)
+#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
+
+import os
+import sys
+import time
+import itertools
+import subprocess
+import numpy as np
+import pybullet
+from multiprocessing import Process
+
+camTargetPos = [0, 0, 0]
+cameraUp = [0, 0, 1]
+cameraPos = [1, 1, 1]
+
+pitch = -10.0
+roll = 0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 84  # 320
+pixelHeight = 84  # 200
+nearPlane = 0.01
+farPlane = 100
+fov = 60
+
+import matplotlib.pyplot as plt
+
+
+class BulletSim():
+
+  def __init__(self, connection_mode, *argv):
+    self.connection_mode = connection_mode
+    self.argv = argv
+
+  def __enter__(self):
+    print("connecting")
+    optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
+    optionstring += ' --window_backend=2 --render_device=0'
+
+    print(self.connection_mode, optionstring, *self.argv)
+    cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
+    if cid < 0:
+      raise ValueError
+    print("connected")
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
+
+    pybullet.resetSimulation()
+    pybullet.loadURDF("plane.urdf", [0, 0, -1])
+    pybullet.loadURDF("r2d2.urdf")
+    pybullet.loadURDF("duck_vhacd.urdf")
+    pybullet.setGravity(0, 0, -10)
+
+  def __exit__(self, *_, **__):
+    pybullet.disconnect()
+
+
+def test(num_runs=300, shadow=1, log=True, plot=False):
+  if log:
+    logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
+
+  if plot:
+    plt.ion()
+
+    img = np.random.rand(200, 320)
+    #img = [tandard_normal((50,100))
+    image = plt.imshow(img, interpolation='none', animated=True, label="blah")
+    ax = plt.gca()
+
+  times = np.zeros(num_runs)
+  yaw_gen = itertools.cycle(range(0, 360, 10))
+  for i, yaw in zip(range(num_runs), yaw_gen):
+    pybullet.stepSimulation()
+    start = time.time()
+    viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
+                                                            roll, upAxisIndex)
+    aspect = pixelWidth / pixelHeight
+    projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
+    img_arr = pybullet.getCameraImage(pixelWidth,
+                                      pixelHeight,
+                                      viewMatrix,
+                                      projectionMatrix,
+                                      shadow=shadow,
+                                      lightDirection=[1, 1, 1],
+                                      renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
+    #renderer=pybullet.ER_TINY_RENDERER)
+    stop = time.time()
+    duration = (stop - start)
+    if (duration):
+      fps = 1. / duration
+      #print("fps=",fps)
+    else:
+      fps = 0
+      #print("fps=",fps)
+    #print("duraction=",duration)
+    #print("fps=",fps)
+    times[i] = fps
+
+    if plot:
+      rgb = img_arr[2]
+      image.set_data(rgb)  #np_img_arr)
+      ax.plot([0])
+      #plt.draw()
+      #plt.show()
+      plt.pause(0.01)
+
+  mean_time = float(np.mean(times))
+  print("mean: {0} for {1} runs".format(mean_time, num_runs))
+  print("")
+  if log:
+    pybullet.stopStateLogging(logId)
+  return mean_time
+
+
+if __name__ == "__main__":
+
+  res = []
+
+  with BulletSim(pybullet.DIRECT):
+    print("\nTesting DIRECT")
+    mean_time = test(log=False, plot=True)
+    res.append(("tiny", mean_time))
+
+  with BulletSim(pybullet.DIRECT):
+    plugin_fn = os.path.join(
+        pybullet.__file__.split("bullet3")[0],
+        "bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
+    plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
+    if plugin < 0:
+      print("\nPlugin Failed to load!\n")
+      sys.exit()
+
+    print("\nTesting DIRECT+OpenGL")
+    mean_time = test(log=True)
+    res.append(("plugin", mean_time))
+
+  with BulletSim(pybullet.GUI):
+    print("\nTesting GUI")
+    mean_time = test(log=False)
+    res.append(("egl", mean_time))
+
+  print()
+  print("rendertest.py")
+  print("back nenv fps fps_tot")
+  for r in res:
+    print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))
diff --git a/examples/pybullet/gym/pybullet_data/laikago/laikago.py b/examples/pybullet/gym/pybullet_data/laikago/laikago.py
index 2a5d8b369..638639f5d 100644
--- a/examples/pybullet/gym/pybullet_data/laikago/laikago.py
+++ b/examples/pybullet/gym/pybullet_data/laikago/laikago.py
@@ -1,110 +1,110 @@
-import pybullet as p

-import time

-

-p.connect(p.GUI)

-plane = p.loadURDF("plane.urdf")

-p.setGravity(0, 0, -9.8)

-p.setTimeStep(1. / 500)

-#p.setDefaultContactERP(0)

-#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS

-urdfFlags = p.URDF_USE_SELF_COLLISION

-quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],

-                       flags=urdfFlags,

-                       useFixedBase=False)

-

-#enable collision between lower legs

-

-for j in range(p.getNumJoints(quadruped)):

-  print(p.getJointInfo(quadruped, j))

-

-#2,5,8 and 11 are the lower legs

-lower_legs = [2, 5, 8, 11]

-for l0 in lower_legs:

-  for l1 in lower_legs:

-    if (l1 > l0):

-      enableCollision = 1

-      print("collision for pair", l0, l1,

-            p.getJointInfo(quadruped, l0)[12],

-            p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)

-      p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)

-

-jointIds = []

-paramIds = []

-jointOffsets = []

-jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]

-jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

-

-for i in range(4):

-  jointOffsets.append(0)

-  jointOffsets.append(-0.7)

-  jointOffsets.append(0.7)

-

-maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)

-

-for j in range(p.getNumJoints(quadruped)):

-  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)

-  info = p.getJointInfo(quadruped, j)

-  #print(info)

-  jointName = info[1]

-  jointType = info[2]

-  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):

-    jointIds.append(j)

-

-p.getCameraImage(480, 320)

-p.setRealTimeSimulation(0)

-

-joints = []

-

-with open("data1.txt", "r") as filestream:

-  for line in filestream:

-    print("line=", line)

-    maxForce = p.readUserDebugParameter(maxForceId)

-    currentline = line.split(",")

-    #print (currentline)

-    #print("-----")

-    frame = currentline[0]

-    t = currentline[1]

-    #print("frame[",frame,"]")

-    joints = currentline[2:14]

-    #print("joints=",joints)

-    for j in range(12):

-      targetPos = float(joints[j])

-      p.setJointMotorControl2(quadruped,

-                              jointIds[j],

-                              p.POSITION_CONTROL,

-                              jointDirections[j] * targetPos + jointOffsets[j],

-                              force=maxForce)

-    p.stepSimulation()

-    for lower_leg in lower_legs:

-      #print("points for ", quadruped, " link: ", lower_leg)

-      pts = p.getContactPoints(quadruped, -1, lower_leg)

-      #print("num points=",len(pts))

-      #for pt in pts:

-      #	print(pt[9])

-    time.sleep(1. / 500.)

-

-for j in range(p.getNumJoints(quadruped)):

-  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)

-  info = p.getJointInfo(quadruped, j)

-  js = p.getJointState(quadruped, j)

-  #print(info)

-  jointName = info[1]

-  jointType = info[2]

-  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):

-    paramIds.append(

-        p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,

-                                (js[0] - jointOffsets[j]) / jointDirections[j]))

-

-p.setRealTimeSimulation(1)

-

-while (1):

-

-  for i in range(len(paramIds)):

-    c = paramIds[i]

-    targetPos = p.readUserDebugParameter(c)

-    maxForce = p.readUserDebugParameter(maxForceId)

-    p.setJointMotorControl2(quadruped,

-                            jointIds[i],

-                            p.POSITION_CONTROL,

-                            jointDirections[i] * targetPos + jointOffsets[i],

-                            force=maxForce)

+import pybullet as p
+import time
+
+p.connect(p.GUI)
+plane = p.loadURDF("plane.urdf")
+p.setGravity(0, 0, -9.8)
+p.setTimeStep(1. / 500)
+#p.setDefaultContactERP(0)
+#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
+urdfFlags = p.URDF_USE_SELF_COLLISION
+quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
+                       flags=urdfFlags,
+                       useFixedBase=False)
+
+#enable collision between lower legs
+
+for j in range(p.getNumJoints(quadruped)):
+  print(p.getJointInfo(quadruped, j))
+
+#2,5,8 and 11 are the lower legs
+lower_legs = [2, 5, 8, 11]
+for l0 in lower_legs:
+  for l1 in lower_legs:
+    if (l1 > l0):
+      enableCollision = 1
+      print("collision for pair", l0, l1,
+            p.getJointInfo(quadruped, l0)[12],
+            p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
+      p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
+
+jointIds = []
+paramIds = []
+jointOffsets = []
+jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
+jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+
+for i in range(4):
+  jointOffsets.append(0)
+  jointOffsets.append(-0.7)
+  jointOffsets.append(0.7)
+
+maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
+
+for j in range(p.getNumJoints(quadruped)):
+  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
+  info = p.getJointInfo(quadruped, j)
+  #print(info)
+  jointName = info[1]
+  jointType = info[2]
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    jointIds.append(j)
+
+p.getCameraImage(480, 320)
+p.setRealTimeSimulation(0)
+
+joints = []
+
+with open("data1.txt", "r") as filestream:
+  for line in filestream:
+    print("line=", line)
+    maxForce = p.readUserDebugParameter(maxForceId)
+    currentline = line.split(",")
+    #print (currentline)
+    #print("-----")
+    frame = currentline[0]
+    t = currentline[1]
+    #print("frame[",frame,"]")
+    joints = currentline[2:14]
+    #print("joints=",joints)
+    for j in range(12):
+      targetPos = float(joints[j])
+      p.setJointMotorControl2(quadruped,
+                              jointIds[j],
+                              p.POSITION_CONTROL,
+                              jointDirections[j] * targetPos + jointOffsets[j],
+                              force=maxForce)
+    p.stepSimulation()
+    for lower_leg in lower_legs:
+      #print("points for ", quadruped, " link: ", lower_leg)
+      pts = p.getContactPoints(quadruped, -1, lower_leg)
+      #print("num points=",len(pts))
+      #for pt in pts:
+      #	print(pt[9])
+    time.sleep(1. / 500.)
+
+for j in range(p.getNumJoints(quadruped)):
+  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
+  info = p.getJointInfo(quadruped, j)
+  js = p.getJointState(quadruped, j)
+  #print(info)
+  jointName = info[1]
+  jointType = info[2]
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    paramIds.append(
+        p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
+                                (js[0] - jointOffsets[j]) / jointDirections[j]))
+
+p.setRealTimeSimulation(1)
+
+while (1):
+
+  for i in range(len(paramIds)):
+    c = paramIds[i]
+    targetPos = p.readUserDebugParameter(c)
+    maxForce = p.readUserDebugParameter(maxForceId)
+    p.setJointMotorControl2(quadruped,
+                            jointIds[i],
+                            p.POSITION_CONTROL,
+                            jointDirections[i] * targetPos + jointOffsets[i],
+                            force=maxForce)
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py
index 1e136c6c4..b6e690fd5 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py
@@ -1 +1 @@
-from . import *

+from . import *
diff --git a/examples/pybullet/gym/pybullet_envs/examples/laikago.py b/examples/pybullet/gym/pybullet_envs/examples/laikago.py
index 081573cc0..d0774761b 100644
--- a/examples/pybullet/gym/pybullet_envs/examples/laikago.py
+++ b/examples/pybullet/gym/pybullet_envs/examples/laikago.py
@@ -1,114 +1,114 @@
-import pybullet as p

-import pybullet_data as pd

-

-import time

-

-p.connect(p.GUI)

-p.setAdditionalSearchPath(pd.getDataPath())

-

-plane = p.loadURDF("plane.urdf")

-p.setGravity(0, 0, -9.8)

-p.setTimeStep(1. / 500)

-#p.setDefaultContactERP(0)

-#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS

-urdfFlags = p.URDF_USE_SELF_COLLISION

-quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],

-                       flags=urdfFlags,

-                       useFixedBase=False)

-

-#enable collision between lower legs

-

-for j in range(p.getNumJoints(quadruped)):

-  print(p.getJointInfo(quadruped, j))

-

-#2,5,8 and 11 are the lower legs

-lower_legs = [2, 5, 8, 11]

-for l0 in lower_legs:

-  for l1 in lower_legs:

-    if (l1 > l0):

-      enableCollision = 1

-      print("collision for pair", l0, l1,

-            p.getJointInfo(quadruped, l0)[12],

-            p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)

-      p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)

-

-jointIds = []

-paramIds = []

-jointOffsets = []

-jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]

-jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

-

-for i in range(4):

-  jointOffsets.append(0)

-  jointOffsets.append(-0.7)

-  jointOffsets.append(0.7)

-

-maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)

-

-for j in range(p.getNumJoints(quadruped)):

-  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)

-  info = p.getJointInfo(quadruped, j)

-  #print(info)

-  jointName = info[1]

-  jointType = info[2]

-  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):

-    jointIds.append(j)

-

-p.getCameraImage(480, 320)

-p.setRealTimeSimulation(0)

-

-joints = []

-

-with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:

-  for line in filestream:

-    print("line=", line)

-    maxForce = p.readUserDebugParameter(maxForceId)

-    currentline = line.split(",")

-    #print (currentline)

-    #print("-----")

-    frame = currentline[0]

-    t = currentline[1]

-    #print("frame[",frame,"]")

-    joints = currentline[2:14]

-    #print("joints=",joints)

-    for j in range(12):

-      targetPos = float(joints[j])

-      p.setJointMotorControl2(quadruped,

-                              jointIds[j],

-                              p.POSITION_CONTROL,

-                              jointDirections[j] * targetPos + jointOffsets[j],

-                              force=maxForce)

-    p.stepSimulation()

-    for lower_leg in lower_legs:

-      #print("points for ", quadruped, " link: ", lower_leg)

-      pts = p.getContactPoints(quadruped, -1, lower_leg)

-      #print("num points=",len(pts))

-      #for pt in pts:

-      # print(pt[9])

-    time.sleep(1. / 500.)

-

-for j in range(p.getNumJoints(quadruped)):

-  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)

-  info = p.getJointInfo(quadruped, j)

-  js = p.getJointState(quadruped, j)

-  #print(info)

-  jointName = info[1]

-  jointType = info[2]

-  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):

-    paramIds.append(

-        p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,

-                                (js[0] - jointOffsets[j]) / jointDirections[j]))

-

-p.setRealTimeSimulation(1)

-

-while (1):

-

-  for i in range(len(paramIds)):

-    c = paramIds[i]

-    targetPos = p.readUserDebugParameter(c)

-    maxForce = p.readUserDebugParameter(maxForceId)

-    p.setJointMotorControl2(quadruped,

-                            jointIds[i],

-                            p.POSITION_CONTROL,

-                            jointDirections[i] * targetPos + jointOffsets[i],

-                            force=maxForce)

+import pybullet as p
+import pybullet_data as pd
+
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+plane = p.loadURDF("plane.urdf")
+p.setGravity(0, 0, -9.8)
+p.setTimeStep(1. / 500)
+#p.setDefaultContactERP(0)
+#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
+urdfFlags = p.URDF_USE_SELF_COLLISION
+quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
+                       flags=urdfFlags,
+                       useFixedBase=False)
+
+#enable collision between lower legs
+
+for j in range(p.getNumJoints(quadruped)):
+  print(p.getJointInfo(quadruped, j))
+
+#2,5,8 and 11 are the lower legs
+lower_legs = [2, 5, 8, 11]
+for l0 in lower_legs:
+  for l1 in lower_legs:
+    if (l1 > l0):
+      enableCollision = 1
+      print("collision for pair", l0, l1,
+            p.getJointInfo(quadruped, l0)[12],
+            p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
+      p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
+
+jointIds = []
+paramIds = []
+jointOffsets = []
+jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
+jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+
+for i in range(4):
+  jointOffsets.append(0)
+  jointOffsets.append(-0.7)
+  jointOffsets.append(0.7)
+
+maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
+
+for j in range(p.getNumJoints(quadruped)):
+  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
+  info = p.getJointInfo(quadruped, j)
+  #print(info)
+  jointName = info[1]
+  jointType = info[2]
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    jointIds.append(j)
+
+p.getCameraImage(480, 320)
+p.setRealTimeSimulation(0)
+
+joints = []
+
+with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
+  for line in filestream:
+    print("line=", line)
+    maxForce = p.readUserDebugParameter(maxForceId)
+    currentline = line.split(",")
+    #print (currentline)
+    #print("-----")
+    frame = currentline[0]
+    t = currentline[1]
+    #print("frame[",frame,"]")
+    joints = currentline[2:14]
+    #print("joints=",joints)
+    for j in range(12):
+      targetPos = float(joints[j])
+      p.setJointMotorControl2(quadruped,
+                              jointIds[j],
+                              p.POSITION_CONTROL,
+                              jointDirections[j] * targetPos + jointOffsets[j],
+                              force=maxForce)
+    p.stepSimulation()
+    for lower_leg in lower_legs:
+      #print("points for ", quadruped, " link: ", lower_leg)
+      pts = p.getContactPoints(quadruped, -1, lower_leg)
+      #print("num points=",len(pts))
+      #for pt in pts:
+      # print(pt[9])
+    time.sleep(1. / 500.)
+
+for j in range(p.getNumJoints(quadruped)):
+  p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
+  info = p.getJointInfo(quadruped, j)
+  js = p.getJointState(quadruped, j)
+  #print(info)
+  jointName = info[1]
+  jointType = info[2]
+  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+    paramIds.append(
+        p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
+                                (js[0] - jointOffsets[j]) / jointDirections[j]))
+
+p.setRealTimeSimulation(1)
+
+while (1):
+
+  for i in range(len(paramIds)):
+    c = paramIds[i]
+    targetPos = p.readUserDebugParameter(c)
+    maxForce = p.readUserDebugParameter(maxForceId)
+    p.setJointMotorControl2(quadruped,
+                            jointIds[i],
+                            p.POSITION_CONTROL,
+                            jointDirections[i] * targetPos + jointOffsets[i],
+                            force=maxForce)
diff --git a/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
index 2446c88cd..c94391d81 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
@@ -1,354 +1,354 @@
-/*

-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

+/*
+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
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
index 5389d05f5..6fedbbb3e 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
@@ -1,1063 +1,1063 @@
-/*

-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

+/*
+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
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 803f6e067..4339b2ea7 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -1,4 +1,4 @@
-/*
+/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
diff --git a/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
index ea946e2da..358bc95d8 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
@@ -1,884 +1,884 @@
-

-/***

- * ---------------------------------

- * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>

- *

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

+
+/***
+ * ---------------------------------
+ * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
+ *
+ *  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
diff --git a/src/BulletCollision/premake4.lua b/src/BulletCollision/premake4.lua
index 6297bf3df..e1c9bdbab 100644
--- a/src/BulletCollision/premake4.lua
+++ b/src/BulletCollision/premake4.lua
@@ -1,23 +1,23 @@
-	project "BulletCollision"

-

-	kind "StaticLib"

-    if os.is("Linux") then

-        buildoptions{"-fPIC"}

-    end

-	includedirs {

-		"..",

-	}

-	files {

-		"*.cpp",

-		"*.h",

-		"BroadphaseCollision/*.cpp",

-		"BroadphaseCollision/*.h",

-		"CollisionDispatch/*.cpp",

-                "CollisionDispatch/*.h",

-		"CollisionShapes/*.cpp",

-		"CollisionShapes/*.h",

-		"Gimpact/*.cpp",

-		"Gimpact/*.h",

-		"NarrowPhaseCollision/*.cpp",

-		"NarrowPhaseCollision/*.h",

-	}

+	project "BulletCollision"
+
+	kind "StaticLib"
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+	includedirs {
+		"..",
+	}
+	files {
+		"*.cpp",
+		"*.h",
+		"BroadphaseCollision/*.cpp",
+		"BroadphaseCollision/*.h",
+		"CollisionDispatch/*.cpp",
+                "CollisionDispatch/*.h",
+		"CollisionShapes/*.cpp",
+		"CollisionShapes/*.h",
+		"Gimpact/*.cpp",
+		"Gimpact/*.h",
+		"NarrowPhaseCollision/*.cpp",
+		"NarrowPhaseCollision/*.h",
+	}
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 7740ffeca..8f6d05a71 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -1,1240 +1,1240 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

-

-This software is provided 'as-is', without any express or implied warranty.

-In no event will the authors be held liable for any damages arising from the use of this software.

-Permission is granted to anyone to use this software for any purpose,

-including commercial applications, and to alter it and redistribute it freely,

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as 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: projectileman@yahoo.com

-http://gimpact.sf.net

-*/

-

-#include "btGeneric6DofSpring2Constraint.h"

-#include "BulletDynamics/Dynamics/btRigidBody.h"

-#include "LinearMath/btTransformUtil.h"

-#include <cmath>

-#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->fps;

-		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;

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

-		btScalar curServoTarget = limot->m_servoTarget;

-		if (rotational)

-		{

-			if (error > SIMD_PI)

-			{

-				error -= SIMD_2_PI;

-				curServoTarget += SIMD_2_PI;

-			}

-			if (error < -SIMD_PI)

-			{

-				error += SIMD_2_PI;

-				curServoTarget -= SIMD_2_PI;

-			}

-		}

-

-		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 ? curServoTarget : -SIMD_INFINITY;

-				hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;

-			}

-			else

-			{

-				lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;

-				hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : 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->fps;

-		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;

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

-		if (rotational)

-		{

-			vel = angVelA.dot(ax1) - angVelB.dot(ax1);

-		}

-		else

-		{

-			btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());

-			btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());

-			vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);

-		}

-		btScalar cfm = BT_ZERO;

-		btScalar mA = BT_ONE / m_rbA.getInvMass();

-		btScalar mB = BT_ONE / m_rbB.getInvMass();

-		if (rotational)

-		{

-			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();

-			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();

-			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();

-			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();

-		}

-		btScalar m;

-		if (m_rbA.getInvMass() == 0) m = mB; else

-		if (m_rbB.getInvMass() == 0) m = mA; else

-			m = mA*mB / (mA + mB);

-		btScalar angularfreq = btSqrt(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);

-

-		// after the spring force affecting the body(es) the new velocity will be

-		// vel + f / m * (rotational ? -1 : 1)

-		// so in theory this should be set here for m_constraintError

-		// (with m_constraintError we set a desired velocity for the affected body(es))

-		// however in practice any value is fine as long as it is greater then the "proper" velocity,

-		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force

-		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)

-		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate

-		// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't

-		// matter, because the solver will limit it according the force limit)

-		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)

-		// will we not request a velocity with the wrong direction ?

-		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError

-		// so the sign of the force that is really matters

-		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);

-

-		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 targetOrg)

-{

-	btAssert((index >= 0) && (index < 6));

-	if (index < 3)

-	{

-		m_linearLimits.m_servoTarget[index] = targetOrg;

-	}

-	else

-	{

-		//wrap between -PI and PI, see also

-		//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi

-

-		btScalar target = targetOrg + SIMD_PI;

-		if (1)

-		{

-			btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);

-			// handle boundary cases resulted from floating-point cut off:

-			{

-				if (m >= SIMD_2_PI)

-				{

-					target = 0;

-				}

-				else

-				{

-					if (m < 0)

-					{

-						if (SIMD_2_PI + m == SIMD_2_PI)

-							target = 0;

-						else

-							target = SIMD_2_PI + m;

-					}

-					else

-					{

-						target = m;

-					}

-				}

-			}

-			target -= SIMD_PI;

-		}

-

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

-	}

-}

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "btGeneric6DofSpring2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <cmath>
+#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->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+		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;
+		btScalar curServoTarget = limot->m_servoTarget;
+		if (rotational)
+		{
+			if (error > SIMD_PI)
+			{
+				error -= SIMD_2_PI;
+				curServoTarget += SIMD_2_PI;
+			}
+			if (error < -SIMD_PI)
+			{
+				error += SIMD_2_PI;
+				curServoTarget -= SIMD_2_PI;
+			}
+		}
+
+		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 ? curServoTarget : -SIMD_INFINITY;
+				hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
+			}
+			else
+			{
+				lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
+				hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : 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->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+		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;
+		if (rotational)
+		{
+			vel = angVelA.dot(ax1) - angVelB.dot(ax1);
+		}
+		else
+		{
+			btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
+			btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
+			vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
+		}
+		btScalar cfm = BT_ZERO;
+		btScalar mA = BT_ONE / m_rbA.getInvMass();
+		btScalar mB = BT_ONE / m_rbB.getInvMass();
+		if (rotational)
+		{
+			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
+			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
+			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
+			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
+		}
+		btScalar m;
+		if (m_rbA.getInvMass() == 0) m = mB; else
+		if (m_rbB.getInvMass() == 0) m = mA; else
+			m = mA*mB / (mA + mB);
+		btScalar angularfreq = btSqrt(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);
+
+		// after the spring force affecting the body(es) the new velocity will be
+		// vel + f / m * (rotational ? -1 : 1)
+		// so in theory this should be set here for m_constraintError
+		// (with m_constraintError we set a desired velocity for the affected body(es))
+		// however in practice any value is fine as long as it is greater then the "proper" velocity,
+		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
+		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
+		// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
+		// matter, because the solver will limit it according the force limit)
+		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
+		// will we not request a velocity with the wrong direction ?
+		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+		// so the sign of the force that is really matters
+		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+
+		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 targetOrg)
+{
+	btAssert((index >= 0) && (index < 6));
+	if (index < 3)
+	{
+		m_linearLimits.m_servoTarget[index] = targetOrg;
+	}
+	else
+	{
+		//wrap between -PI and PI, see also
+		//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
+
+		btScalar target = targetOrg + SIMD_PI;
+		if (1)
+		{
+			btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
+			// handle boundary cases resulted from floating-point cut off:
+			{
+				if (m >= SIMD_2_PI)
+				{
+					target = 0;
+				}
+				else
+				{
+					if (m < 0)
+					{
+						if (SIMD_2_PI + m == SIMD_2_PI)
+							target = 0;
+						else
+							target = SIMD_2_PI + m;
+					}
+					else
+					{
+						target = m;
+					}
+				}
+			}
+			target -= SIMD_PI;
+		}
+
+		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;
+	}
+}
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index 4d15aaf35..bc3ee6d21 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -1,666 +1,666 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

-

-This software is provided 'as-is', without any express or implied warranty.

-In no event will the authors be held liable for any damages arising from the use of this software.

-Permission is granted to anyone to use this software for any purpose,

-including commercial applications, and to alter it and redistribute it freely,

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as 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: projectileman@yahoo.com

-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 = 6.0f;

-		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);

-

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

-

-	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);

-};

-

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

-

-	dof->m_padding1[0] = 0;

-	dof->m_padding1[1] = 0;

-	dof->m_padding1[2] = 0;

-	dof->m_padding1[3] = 0;

-

-	return btGeneric6DofSpring2ConstraintDataName;

-}

-

-#endif  //BT_GENERIC_6DOF_CONSTRAINT_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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: projectileman@yahoo.com
+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 = 6.0f;
+		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);
+
+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;
+
+	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);
+};
+
+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;
+
+	dof->m_padding1[0] = 0;
+	dof->m_padding1[1] = 0;
+	dof->m_padding1[2] = 0;
+	dof->m_padding1[3] = 0;
+
+	return btGeneric6DofSpring2ConstraintDataName;
+}
+
+#endif  //BT_GENERIC_6DOF_CONSTRAINT_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 8df15b58c..d4f8a42c5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -1,2109 +1,2109 @@
-/*

- * PURPOSE:

- *   Class representing an articulated rigid body. Stores the body's

- *   current state, allows forces and torques to be set, handles

- *   timestepping and implements Featherstone's algorithm.

- *   

- * COPYRIGHT:

- *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013

- *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)

- *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements

-

- This software is provided 'as-is', without any express or implied warranty.

- In no event will the authors be held liable for any damages arising from the use of this software.

- Permission is granted to anyone to use this software for any purpose,

- including commercial applications, and to alter it and redistribute it freely,

- subject to the following restrictions:

- 

- 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

- 2. Altered source versions must be plainly marked as 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 "btMultiBody.h"

-#include "btMultiBodyLink.h"

-#include "btMultiBodyLinkCollider.h"

-#include "btMultiBodyJointFeedback.h"

-#include "LinearMath/btTransformUtil.h"

-#include "LinearMath/btSerializer.h"

-//#include "Bullet3Common/b3Logging.h"

-// #define INCLUDE_GYRO_TERM

-

-

-namespace

-{

-const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)

-const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds

-}  // namespace

-

-void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame

-	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates

-	const btVector3 &top_in,             // top part of input vector

-	const btVector3 &bottom_in,          // bottom part of input vector

-	btVector3 &top_out,                  // top part of output vector

-	btVector3 &bottom_out)               // bottom part of output vector

-{

-	top_out = rotation_matrix * top_in;

-	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;

-}

-

-namespace

-{

-

-

-#if 0

-    void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,

-                                 const btVector3 &displacement,

-                                 const btVector3 &top_in,

-                                 const btVector3 &bottom_in,

-                                 btVector3 &top_out,

-                                 btVector3 &bottom_out)

-    {

-        top_out = rotation_matrix.transpose() * top_in;

-        bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));		

-    }

-

-    btScalar SpatialDotProduct(const btVector3 &a_top,

-                            const btVector3 &a_bottom,

-                            const btVector3 &b_top,

-                            const btVector3 &b_bottom)

-    {

-        return a_bottom.dot(b_top) + a_top.dot(b_bottom);

-    }

-

-	void SpatialCrossProduct(const btVector3 &a_top,

-                            const btVector3 &a_bottom,

-                            const btVector3 &b_top,

-                            const btVector3 &b_bottom,

-							btVector3 &top_out,

-							btVector3 &bottom_out)

-	{

-		top_out = a_top.cross(b_top);

-		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);

-	}

-#endif

-

-}  // namespace

-

-//

-// Implementation of class btMultiBody

-//

-

-btMultiBody::btMultiBody(int n_links,

-						 btScalar mass,

-						 const btVector3 &inertia,

-						 bool fixedBase,

-						 bool canSleep,

-						 bool /*deprecatedUseMultiDof*/)

-	: m_baseCollider(0),

-	  m_baseName(0),

-	  m_basePos(0, 0, 0),

-	  m_baseQuat(0, 0, 0, 1),

-	  m_baseMass(mass),

-	  m_baseInertia(inertia),

-

-	  m_fixedBase(fixedBase),

-	  m_awake(true),

-	  m_canSleep(canSleep),

-	  m_canWakeup(true),

-	  m_sleepTimer(0),

-	  m_userObjectPointer(0),

-	  m_userIndex2(-1),

-	  m_userIndex(-1),

-	  m_companionId(-1),

-	  m_linearDamping(0.04f),

-	  m_angularDamping(0.04f),

-	  m_useGyroTerm(true),

-	  m_maxAppliedImpulse(1000.f),

-	  m_maxCoordinateVelocity(100.f),

-	  m_hasSelfCollision(true),

-	  __posUpdated(false),

-	  m_dofCount(0),

-	  m_posVarCnt(0),

-	  m_useRK4(false),

-	  m_useGlobalVelocities(false),

-	  m_internalNeedsJointFeedback(false)

-{

-	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);

-	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);

-	m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);

-	m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);

-	m_cachedInertiaValid = false;

-

-	m_links.resize(n_links);

-	m_matrixBuf.resize(n_links + 1);

-

-	m_baseForce.setValue(0, 0, 0);

-	m_baseTorque.setValue(0, 0, 0);

-

-	clearConstraintForces();

-	clearForcesAndTorques();

-}

-

-btMultiBody::~btMultiBody()

-{

-}

-

-void btMultiBody::setupFixed(int i,

-							 btScalar mass,

-							 const btVector3 &inertia,

-							 int parent,

-							 const btQuaternion &rotParentToThis,

-							 const btVector3 &parentComToThisPivotOffset,

-							 const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)

-{

-	m_links[i].m_mass = mass;

-	m_links[i].m_inertiaLocal = inertia;

-	m_links[i].m_parent = parent;

-	m_links[i].setAxisTop(0, 0., 0., 0.);

-	m_links[i].setAxisBottom(0, btVector3(0, 0, 0));

-	m_links[i].m_zeroRotParentToThis = rotParentToThis;

-	m_links[i].m_dVector = thisPivotToThisComOffset;

-	m_links[i].m_eVector = parentComToThisPivotOffset;

-

-	m_links[i].m_jointType = btMultibodyLink::eFixed;

-	m_links[i].m_dofCount = 0;

-	m_links[i].m_posVarCount = 0;

-

-	m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

-

-	m_links[i].updateCacheMultiDof();

-

-	updateLinksDofOffsets();

-}

-

-void btMultiBody::setupPrismatic(int i,

-								 btScalar mass,

-								 const btVector3 &inertia,

-								 int parent,

-								 const btQuaternion &rotParentToThis,

-								 const btVector3 &jointAxis,

-								 const btVector3 &parentComToThisPivotOffset,

-								 const btVector3 &thisPivotToThisComOffset,

-								 bool disableParentCollision)

-{

-	m_dofCount += 1;

-	m_posVarCnt += 1;

-

-	m_links[i].m_mass = mass;

-	m_links[i].m_inertiaLocal = inertia;

-	m_links[i].m_parent = parent;

-	m_links[i].m_zeroRotParentToThis = rotParentToThis;

-	m_links[i].setAxisTop(0, 0., 0., 0.);

-	m_links[i].setAxisBottom(0, jointAxis);

-	m_links[i].m_eVector = parentComToThisPivotOffset;

-	m_links[i].m_dVector = thisPivotToThisComOffset;

-	m_links[i].m_cachedRotParentToThis = rotParentToThis;

-

-	m_links[i].m_jointType = btMultibodyLink::ePrismatic;

-	m_links[i].m_dofCount = 1;

-	m_links[i].m_posVarCount = 1;

-	m_links[i].m_jointPos[0] = 0.f;

-	m_links[i].m_jointTorque[0] = 0.f;

-

-	if (disableParentCollision)

-		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

-	//

-

-	m_links[i].updateCacheMultiDof();

-

-	updateLinksDofOffsets();

-}

-

-void btMultiBody::setupRevolute(int i,

-								btScalar mass,

-								const btVector3 &inertia,

-								int parent,

-								const btQuaternion &rotParentToThis,

-								const btVector3 &jointAxis,

-								const btVector3 &parentComToThisPivotOffset,

-								const btVector3 &thisPivotToThisComOffset,

-								bool disableParentCollision)

-{

-	m_dofCount += 1;

-	m_posVarCnt += 1;

-

-	m_links[i].m_mass = mass;

-	m_links[i].m_inertiaLocal = inertia;

-	m_links[i].m_parent = parent;

-	m_links[i].m_zeroRotParentToThis = rotParentToThis;

-	m_links[i].setAxisTop(0, jointAxis);

-	m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));

-	m_links[i].m_dVector = thisPivotToThisComOffset;

-	m_links[i].m_eVector = parentComToThisPivotOffset;

-

-	m_links[i].m_jointType = btMultibodyLink::eRevolute;

-	m_links[i].m_dofCount = 1;

-	m_links[i].m_posVarCount = 1;

-	m_links[i].m_jointPos[0] = 0.f;

-	m_links[i].m_jointTorque[0] = 0.f;

-

-	if (disableParentCollision)

-		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

-	//

-	m_links[i].updateCacheMultiDof();

-	//

-	updateLinksDofOffsets();

-}

-

-void btMultiBody::setupSpherical(int i,

-								 btScalar mass,

-								 const btVector3 &inertia,

-								 int parent,

-								 const btQuaternion &rotParentToThis,

-								 const btVector3 &parentComToThisPivotOffset,

-								 const btVector3 &thisPivotToThisComOffset,

-								 bool disableParentCollision)

-{

-	m_dofCount += 3;

-	m_posVarCnt += 4;

-

-	m_links[i].m_mass = mass;

-	m_links[i].m_inertiaLocal = inertia;

-	m_links[i].m_parent = parent;

-	m_links[i].m_zeroRotParentToThis = rotParentToThis;

-	m_links[i].m_dVector = thisPivotToThisComOffset;

-	m_links[i].m_eVector = parentComToThisPivotOffset;

-

-	m_links[i].m_jointType = btMultibodyLink::eSpherical;

-	m_links[i].m_dofCount = 3;

-	m_links[i].m_posVarCount = 4;

-	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);

-	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);

-	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);

-	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));

-	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));

-	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));

-	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;

-	m_links[i].m_jointPos[3] = 1.f;

-	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;

-

-	if (disableParentCollision)

-		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

-	//

-	m_links[i].updateCacheMultiDof();

-	//

-	updateLinksDofOffsets();

-}

-

-void btMultiBody::setupPlanar(int i,

-							  btScalar mass,

-							  const btVector3 &inertia,

-							  int parent,

-							  const btQuaternion &rotParentToThis,

-							  const btVector3 &rotationAxis,

-							  const btVector3 &parentComToThisComOffset,

-							  bool disableParentCollision)

-{

-	m_dofCount += 3;

-	m_posVarCnt += 3;

-

-	m_links[i].m_mass = mass;

-	m_links[i].m_inertiaLocal = inertia;

-	m_links[i].m_parent = parent;

-	m_links[i].m_zeroRotParentToThis = rotParentToThis;

-	m_links[i].m_dVector.setZero();

-	m_links[i].m_eVector = parentComToThisComOffset;

-

-	//

-	btVector3 vecNonParallelToRotAxis(1, 0, 0);

-	if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)

-		vecNonParallelToRotAxis.setValue(0, 1, 0);

-	//

-

-	m_links[i].m_jointType = btMultibodyLink::ePlanar;

-	m_links[i].m_dofCount = 3;

-	m_links[i].m_posVarCount = 3;

-	btVector3 n = rotationAxis.normalized();

-	m_links[i].setAxisTop(0, n[0], n[1], n[2]);

-	m_links[i].setAxisTop(1, 0, 0, 0);

-	m_links[i].setAxisTop(2, 0, 0, 0);

-	m_links[i].setAxisBottom(0, 0, 0, 0);

-	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);

-	m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);

-	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));

-	m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);

-	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;

-	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;

-

-	if (disableParentCollision)

-		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;

-	//

-	m_links[i].updateCacheMultiDof();

-	//

-	updateLinksDofOffsets();

-

-	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());

-	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());

-}

-

-void btMultiBody::finalizeMultiDof()

-{

-	m_deltaV.resize(0);

-	m_deltaV.resize(6 + m_dofCount);

-	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")

-	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)

-	m_matrixBuf.resize(m_links.size() + 1);

-	for (int i = 0; i < m_vectorBuf.size(); i++)

-	{

-		m_vectorBuf[i].setValue(0, 0, 0);

-	}

-	updateLinksDofOffsets();

-}

-

-int btMultiBody::getParent(int i) const

-{

-	return m_links[i].m_parent;

-}

-

-btScalar btMultiBody::getLinkMass(int i) const

-{

-	return m_links[i].m_mass;

-}

-

-const btVector3 &btMultiBody::getLinkInertia(int i) const

-{

-	return m_links[i].m_inertiaLocal;

-}

-

-btScalar btMultiBody::getJointPos(int i) const

-{

-	return m_links[i].m_jointPos[0];

-}

-

-btScalar btMultiBody::getJointVel(int i) const

-{

-	return m_realBuf[6 + m_links[i].m_dofOffset];

-}

-

-btScalar *btMultiBody::getJointPosMultiDof(int i)

-{

-	return &m_links[i].m_jointPos[0];

-}

-

-btScalar *btMultiBody::getJointVelMultiDof(int i)

-{

-	return &m_realBuf[6 + m_links[i].m_dofOffset];

-}

-

-const btScalar *btMultiBody::getJointPosMultiDof(int i) const

-{

-	return &m_links[i].m_jointPos[0];

-}

-

-const btScalar *btMultiBody::getJointVelMultiDof(int i) const

-{

-	return &m_realBuf[6 + m_links[i].m_dofOffset];

-}

-

-void btMultiBody::setJointPos(int i, btScalar q)

-{

-	m_links[i].m_jointPos[0] = q;

-	m_links[i].updateCacheMultiDof();

-}

-

-

-void btMultiBody::setJointPosMultiDof(int i, const double *q)

-{

-	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)

-		m_links[i].m_jointPos[pos] = (btScalar)q[pos];

-

-	m_links[i].updateCacheMultiDof();

-}

-

-void btMultiBody::setJointPosMultiDof(int i, const float *q)

-{

-	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)

-		m_links[i].m_jointPos[pos] = (btScalar)q[pos];

-

-	m_links[i].updateCacheMultiDof();

-}

-

-

-

-void btMultiBody::setJointVel(int i, btScalar qdot)

-{

-	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;

-}

-

-void btMultiBody::setJointVelMultiDof(int i, const double *qdot)

-{

-	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];

-}

-

-void btMultiBody::setJointVelMultiDof(int i, const float* qdot)

-{

-	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];

-}

-

-const btVector3 &btMultiBody::getRVector(int i) const

-{

-	return m_links[i].m_cachedRVector;

-}

-

-const btQuaternion &btMultiBody::getParentToLocalRot(int i) const

-{

-	return m_links[i].m_cachedRotParentToThis;

-}

-

-btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const

-{

-	btAssert(i >= -1);

-	btAssert(i < m_links.size());

-	if ((i < -1) || (i >= m_links.size()))

-	{

-		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);

-	}

-

-	btVector3 result = local_pos;

-	while (i != -1)

-	{

-		// 'result' is in frame i. transform it to frame parent(i)

-		result += getRVector(i);

-		result = quatRotate(getParentToLocalRot(i).inverse(), result);

-		i = getParent(i);

-	}

-

-	// 'result' is now in the base frame. transform it to world frame

-	result = quatRotate(getWorldToBaseRot().inverse(), result);

-	result += getBasePos();

-

-	return result;

-}

-

-btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const

-{

-	btAssert(i >= -1);

-	btAssert(i < m_links.size());

-	if ((i < -1) || (i >= m_links.size()))

-	{

-		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);

-	}

-

-	if (i == -1)

-	{

-		// world to base

-		return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));

-	}

-	else

-	{

-		// find position in parent frame, then transform to current frame

-		return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);

-	}

-}

-

-btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const

-{

-	btAssert(i >= -1);

-	btAssert(i < m_links.size());

-	if ((i < -1) || (i >= m_links.size()))

-	{

-		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);

-	}

-

-	btVector3 result = local_dir;

-	while (i != -1)

-	{

-		result = quatRotate(getParentToLocalRot(i).inverse(), result);

-		i = getParent(i);

-	}

-	result = quatRotate(getWorldToBaseRot().inverse(), result);

-	return result;

-}

-

-btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const

-{

-	btAssert(i >= -1);

-	btAssert(i < m_links.size());

-	if ((i < -1) || (i >= m_links.size()))

-	{

-		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);

-	}

-

-	if (i == -1)

-	{

-		return quatRotate(getWorldToBaseRot(), world_dir);

-	}

-	else

-	{

-		return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));

-	}

-}

-

-btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const

-{

-	btMatrix3x3 result = local_frame;

-	btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));

-	btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));

-	btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));

-	result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);

-	return result;

-}

-

-void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const

-{

-	int num_links = getNumLinks();

-	// Calculates the velocities of each link (and the base) in its local frame

-	const btQuaternion& base_rot = getWorldToBaseRot();

-	omega[0] = quatRotate(base_rot, getBaseOmega());

-	vel[0] = quatRotate(base_rot, getBaseVel());

-

-	for (int i = 0; i < num_links; ++i)

-	{

-		const btMultibodyLink& link = getLink(i);

-		const int parent = link.m_parent;

-

-		// transform parent vel into this frame, store in omega[i+1], vel[i+1]

-		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,

-			omega[parent + 1], vel[parent + 1],

-			omega[i + 1], vel[i + 1]);

-

-		// now add qidot * shat_i

-		const btScalar* jointVel = getJointVelMultiDof(i);

-		for (int dof = 0; dof < link.m_dofCount; ++dof)

-		{

-			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);

-			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);

-		}

-	}

-}

-

-btScalar btMultiBody::getKineticEnergy() const

-{

-	int num_links = getNumLinks();

-	// TODO: would be better not to allocate memory here

-	btAlignedObjectArray<btVector3> omega;

-	omega.resize(num_links + 1);

-	btAlignedObjectArray<btVector3> vel;

-	vel.resize(num_links + 1);

-	compTreeLinkVelocities(&omega[0], &vel[0]);

-

-	// we will do the factor of 0.5 at the end

-	btScalar result = m_baseMass * vel[0].dot(vel[0]);

-	result += omega[0].dot(m_baseInertia * omega[0]);

-

-	for (int i = 0; i < num_links; ++i)

-	{

-		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);

-		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);

-	}

-

-	return 0.5f * result;

-}

-

-btVector3 btMultiBody::getAngularMomentum() const

-{

-	int num_links = getNumLinks();

-	// TODO: would be better not to allocate memory here

-	btAlignedObjectArray<btVector3> omega;

-	omega.resize(num_links + 1);

-	btAlignedObjectArray<btVector3> vel;

-	vel.resize(num_links + 1);

-	btAlignedObjectArray<btQuaternion> rot_from_world;

-	rot_from_world.resize(num_links + 1);

-	compTreeLinkVelocities(&omega[0], &vel[0]);

-

-	rot_from_world[0] = m_baseQuat;

-	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));

-

-	for (int i = 0; i < num_links; ++i)

-	{

-		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];

-		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));

-	}

-

-	return result;

-}

-

-void btMultiBody::clearConstraintForces()

-{

-	m_baseConstraintForce.setValue(0, 0, 0);

-	m_baseConstraintTorque.setValue(0, 0, 0);

-

-	for (int i = 0; i < getNumLinks(); ++i)

-	{

-		m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);

-		m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);

-	}

-}

-void btMultiBody::clearForcesAndTorques()

-{

-	m_baseForce.setValue(0, 0, 0);

-	m_baseTorque.setValue(0, 0, 0);

-

-	for (int i = 0; i < getNumLinks(); ++i)

-	{

-		m_links[i].m_appliedForce.setValue(0, 0, 0);

-		m_links[i].m_appliedTorque.setValue(0, 0, 0);

-		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;

-	}

-}

-

-void btMultiBody::clearVelocities()

-{

-	for (int i = 0; i < 6 + getNumDofs(); ++i)

-	{

-		m_realBuf[i] = 0.f;

-	}

-}

-void btMultiBody::addLinkForce(int i, const btVector3 &f)

-{

-	m_links[i].m_appliedForce += f;

-}

-

-void btMultiBody::addLinkTorque(int i, const btVector3 &t)

-{

-	m_links[i].m_appliedTorque += t;

-}

-

-void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)

-{

-	m_links[i].m_appliedConstraintForce += f;

-}

-

-void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)

-{

-	m_links[i].m_appliedConstraintTorque += t;

-}

-

-void btMultiBody::addJointTorque(int i, btScalar Q)

-{

-	m_links[i].m_jointTorque[0] += Q;

-}

-

-void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)

-{

-	m_links[i].m_jointTorque[dof] += Q;

-}

-

-void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)

-{

-	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		m_links[i].m_jointTorque[dof] = Q[dof];

-}

-

-const btVector3 &btMultiBody::getLinkForce(int i) const

-{

-	return m_links[i].m_appliedForce;

-}

-

-const btVector3 &btMultiBody::getLinkTorque(int i) const

-{

-	return m_links[i].m_appliedTorque;

-}

-

-btScalar btMultiBody::getJointTorque(int i) const

-{

-	return m_links[i].m_jointTorque[0];

-}

-

-btScalar *btMultiBody::getJointTorqueMultiDof(int i)

-{

-	return &m_links[i].m_jointTorque[0];

-}

-

-inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?

-{

-	btVector3 row0 = btVector3(

-		v0.x() * v1.x(),

-		v0.x() * v1.y(),

-		v0.x() * v1.z());

-	btVector3 row1 = btVector3(

-		v0.y() * v1.x(),

-		v0.y() * v1.y(),

-		v0.y() * v1.z());

-	btVector3 row2 = btVector3(

-		v0.z() * v1.x(),

-		v0.z() * v1.y(),

-		v0.z() * v1.z());

-

-	btMatrix3x3 m(row0[0], row0[1], row0[2],

-				  row1[0], row1[1], row1[2],

-				  row2[0], row2[1], row2[2]);

-	return m;

-}

-

-#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)

-//

-

-void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,

-    btAlignedObjectArray<btScalar> &scratch_r,

-    btAlignedObjectArray<btVector3> &scratch_v,

-    btAlignedObjectArray<btMatrix3x3> &scratch_m,

-	bool isConstraintPass,

-	bool jointFeedbackInWorldSpace,

-	bool jointFeedbackInJointFrame)

-{

-	// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)

-	// and the base linear & angular accelerations.

-

-	// We apply damping forces in this routine as well as any external forces specified by the

-	// caller (via addBaseForce etc).

-

-	// output should point to an array of 6 + num_links reals.

-	// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),

-	// num_links joint acceleration values.

-

-	// We added support for multi degree of freedom (multi dof) joints.

-	// In addition we also can compute the joint reaction forces. This is performed in a second pass,

-	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)

-

-	m_internalNeedsJointFeedback = false;

-

-	int num_links = getNumLinks();

-

-	const btScalar DAMPING_K1_LINEAR = m_linearDamping;

-	const btScalar DAMPING_K2_LINEAR = m_linearDamping;

-

-	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;

-	const btScalar DAMPING_K2_ANGULAR = m_angularDamping;

-

-	const btVector3 base_vel = getBaseVel();

-	const btVector3 base_omega = getBaseOmega();

-

-	// Temporary matrices/vectors -- use scratch space from caller

-	// so that we don't have to keep reallocating every frame

-

-	scratch_r.resize(2 * m_dofCount + 7);  //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount

-	scratch_v.resize(8 * num_links + 6);

-	scratch_m.resize(4 * num_links + 4);

-

-	//btScalar * r_ptr = &scratch_r[0];

-	btScalar *output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results

-	btVector3 *v_ptr = &scratch_v[0];

-

-	// vhat_i  (top = angular, bottom = linear part)

-	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;

-	v_ptr += num_links * 2 + 2;

-	//

-	// zhat_i^A

-	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;

-	v_ptr += num_links * 2 + 2;

-	//

-	// chat_i  (note NOT defined for the base)

-	btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;

-	v_ptr += num_links * 2;

-	//

-	// Ihat_i^A.

-	btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];

-

-	// Cached 3x3 rotation matrices from parent frame to this frame.

-	btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];

-	btMatrix3x3 *rot_from_world = &scratch_m[0];

-

-	// hhat_i, ahat_i

-	// hhat is NOT stored for the base (but ahat is)

-	btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);

-	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;

-	v_ptr += num_links * 2 + 2;

-	//

-	// Y_i, invD_i

-	btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;

-	btScalar *Y = &scratch_r[0];

-	//

-	//aux variables

-	btSpatialMotionVector spatJointVel;         //spatial velocity due to the joint motion (i.e. without predecessors' influence)

-	btScalar D[36];                             //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do

-	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies

-	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)

-	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough

-	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors

-	btSpatialTransformationMatrix fromParent;   //spatial transform from parent to child

-	btSymmetricSpatialDyad dyadTemp;            //inertia matrix temp

-	btSpatialTransformationMatrix fromWorld;

-	fromWorld.m_trnVec.setZero();

-	/////////////////

-

-	// ptr to the joint accel part of the output

-	btScalar *joint_accel = output + 6;

-

-	// Start of the algorithm proper.

-

-	// First 'upward' loop.

-	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

-

-	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?

-

-	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates

-	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);

-

-	if (m_fixedBase)

-	{

-		zeroAccSpatFrc[0].setZero();

-	}

-	else

-	{

-		const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;

-		const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;

-		//external forces

-		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));

-

-		//adding damping terms (only)

-		const btScalar linDampMult = 1., angDampMult = 1.;

-		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),

-									linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));

-

-		//

-		//p += vhat x Ihat vhat - done in a simpler way

-		if (m_useGyroTerm)

-			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));

-		//

-		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));

-	}

-

-	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)

-	spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),

-							 //

-							 btMatrix3x3(m_baseMass, 0, 0,

-										 0, m_baseMass, 0,

-										 0, 0, m_baseMass),

-							 //

-							 btMatrix3x3(m_baseInertia[0], 0, 0,

-										 0, m_baseInertia[1], 0,

-										 0, 0, m_baseInertia[2]));

-

-	rot_from_world[0] = rot_from_parent[0];

-

-	//

-	for (int i = 0; i < num_links; ++i)

-	{

-		const int parent = m_links[i].m_parent;

-		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);

-		rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];

-

-		fromParent.m_rotMat = rot_from_parent[i + 1];

-		fromParent.m_trnVec = m_links[i].m_cachedRVector;

-		fromWorld.m_rotMat = rot_from_world[i + 1];

-		fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);

-

-		// now set vhat_i to its true value by doing

-		// vhat_i += qidot * shat_i

-		if (!m_useGlobalVelocities)

-		{

-			spatJointVel.setZero();

-

-			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];

-

-			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint

-			spatVel[i + 1] += spatJointVel;

-

-			//

-			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint

-			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;

-		}

-		else

-		{

-			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);

-			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);

-		}

-

-		// we can now calculate chat_i

-		spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);

-

-		// calculate zhat_i^A

-		//

-		//external forces

-		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;

-		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;

-

-		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));

-

-#if 0	

-		{

-

-			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",

-			i+1,

-			zeroAccSpatFrc[i+1].m_topVec[0],

-			zeroAccSpatFrc[i+1].m_topVec[1],

-			zeroAccSpatFrc[i+1].m_topVec[2],

-

-			zeroAccSpatFrc[i+1].m_bottomVec[0],

-			zeroAccSpatFrc[i+1].m_bottomVec[1],

-			zeroAccSpatFrc[i+1].m_bottomVec[2]);

-		}

-#endif

-		//

-		//adding damping terms (only)

-		btScalar linDampMult = 1., angDampMult = 1.;

-		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),

-										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));

-

-		// calculate Ihat_i^A

-		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)

-		spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),

-									 //

-									 btMatrix3x3(m_links[i].m_mass, 0, 0,

-												 0, m_links[i].m_mass, 0,

-												 0, 0, m_links[i].m_mass),

-									 //

-									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,

-												 0, m_links[i].m_inertiaLocal[1], 0,

-												 0, 0, m_links[i].m_inertiaLocal[2]));

-		//

-		//p += vhat x Ihat vhat - done in a simpler way

-		if (m_useGyroTerm)

-			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));

-		//

-		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));

-		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());

-		////clamp parent's omega

-		//btScalar parOmegaMod = temp.length();

-		//btScalar parOmegaModMax = 1000;

-		//if(parOmegaMod > parOmegaModMax)

-		//	temp *= parOmegaModMax / parOmegaMod;

-		//zeroAccSpatFrc[i+1].addLinear(temp);

-		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());

-		//temp = spatCoriolisAcc[i].getLinear();

-		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());

-

-		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());

-		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());

-		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());

-	}

-

-	// 'Downward' loop.

-	// (part of TreeForwardDynamics in Mirtich.)

-	for (int i = num_links - 1; i >= 0; --i)

-	{

-		const int parent = m_links[i].m_parent;

-		fromParent.m_rotMat = rot_from_parent[i + 1];

-		fromParent.m_trnVec = m_links[i].m_cachedRVector;

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];

-			//

-			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);

-		}

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];

-			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)

-			{

-				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];

-				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);

-			}

-		}

-

-		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];

-		switch (m_links[i].m_jointType)

-		{

-			case btMultibodyLink::ePrismatic:

-			case btMultibodyLink::eRevolute:

-			{

-				if (D[0] >= SIMD_EPSILON)

-				{

-					invDi[0] = 1.0f / D[0];

-				}

-				else

-				{

-					invDi[0] = 0;

-				}

-				break;

-			}

-			case btMultibodyLink::eSpherical:

-			case btMultibodyLink::ePlanar:

-			{

-				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);

-				const btMatrix3x3 invD3x3(D3x3.inverse());

-

-				//unroll the loop?

-				for (int row = 0; row < 3; ++row)

-				{

-					for (int col = 0; col < 3; ++col)

-					{

-						invDi[row * 3 + col] = invD3x3[row][col];

-					}

-				}

-

-				break;

-			}

-			default:

-			{

-			}

-		}

-

-		//determine h*D^{-1}

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			spatForceVecTemps[dof].setZero();

-

-			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)

-			{

-				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];

-				//

-				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];

-			}

-		}

-

-		dyadTemp = spatInertia[i + 1];

-

-		//determine (h*D^{-1}) * h^{T}

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);

-		}

-

-		fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			invD_times_Y[dof] = 0.f;

-

-			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)

-			{

-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];

-			}

-		}

-

-		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];

-		}

-

-		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);

-

-		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];

-	}

-

-	// Second 'upward' loop

-	// (part of TreeForwardDynamics in Mirtich)

-

-	if (m_fixedBase)

-	{

-		spatAcc[0].setZero();

-	}

-	else

-	{

-		if (num_links > 0)

-		{

-			m_cachedInertiaValid = true;

-			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;

-			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;

-			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;

-			m_cachedInertiaLowerRight = spatInertia[0].m_topLeftMat.transpose();

-		}

-

-		solveImatrix(zeroAccSpatFrc[0], result);

-		spatAcc[0] = -result;

-	}

-

-	// now do the loop over the m_links

-	for (int i = 0; i < num_links; ++i)

-	{

-		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)

-		//	a = apar + cor + Sqdd

-		//or

-		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))

-		//	a = apar + Sqdd

-

-		const int parent = m_links[i].m_parent;

-		fromParent.m_rotMat = rot_from_parent[i + 1];

-		fromParent.m_trnVec = m_links[i].m_cachedRVector;

-

-		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);

-		}

-

-		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];

-		//D^{-1} * (Y - h^{T}*apar)

-		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);

-

-		spatAcc[i + 1] += spatCoriolisAcc[i];

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];

-

-		if (m_links[i].m_jointFeedback)

-		{

-			m_internalNeedsJointFeedback = true;

-

-			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;

-			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;

-

-			if (jointFeedbackInJointFrame)

-			{

-				//shift the reaction forces to the joint frame

-				//linear (force) component is the same

-				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector

-				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);

-			}

-

-			if (jointFeedbackInWorldSpace)

-			{

-				if (isConstraintPass)

-				{

-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;

-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;

-				}

-				else

-				{

-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;

-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;

-				}

-			}

-			else

-			{

-				if (isConstraintPass)

-				{

-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;

-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;

-				}

-				else

-				{

-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;

-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;

-				}

-			}

-		}

-	}

-

-	// transform base accelerations back to the world frame.

-	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();

-	output[0] = omegadot_out[0];

-	output[1] = omegadot_out[1];

-	output[2] = omegadot_out[2];

-

-	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));

-	output[3] = vdot_out[0];

-	output[4] = vdot_out[1];

-	output[5] = vdot_out[2];

-

-	/////////////////

-	//printf("q = [");

-	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());

-	//for(int link = 0; link < getNumLinks(); ++link)

-	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)

-	//		printf("%.6f ", m_links[link].m_jointPos[dof]);

-	//printf("]\n");

-	////

-	//printf("qd = [");

-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)

-	//	printf("%.6f ", m_realBuf[dof]);

-	//printf("]\n");

-	//printf("qdd = [");

-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)

-	//	printf("%.6f ", output[dof]);

-	//printf("]\n");

-	/////////////////

-

-	// Final step: add the accelerations (times dt) to the velocities.

-

-	if (!isConstraintPass)

-	{

-		if (dt > 0.)

-			applyDeltaVeeMultiDof(output, dt);

-	}

-	/////

-	//btScalar angularThres = 1;

-	//btScalar maxAngVel = 0.;

-	//bool scaleDown = 1.;

-	//for(int link = 0; link < m_links.size(); ++link)

-	//{

-	//	if(spatVel[link+1].getAngular().length() > maxAngVel)

-	//	{

-	//		maxAngVel = spatVel[link+1].getAngular().length();

-	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();

-	//		break;

-	//	}

-	//}

-

-	//if(scaleDown != 1.)

-	//{

-	//	for(int link = 0; link < m_links.size(); ++link)

-	//	{

-	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)

-	//		{

-	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)

-	//				getJointVelMultiDof(link)[dof] *= scaleDown;

-	//		}

-	//	}

-	//}

-	/////

-

-	/////////////////////

-	if (m_useGlobalVelocities)

-	{

-		for (int i = 0; i < num_links; ++i)

-		{

-			const int parent = m_links[i].m_parent;

-			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done

-			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done

-

-			fromParent.m_rotMat = rot_from_parent[i + 1];

-			fromParent.m_trnVec = m_links[i].m_cachedRVector;

-			fromWorld.m_rotMat = rot_from_world[i + 1];

-

-			// vhat_i = i_xhat_p(i) * vhat_p(i)

-			fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);

-			//nice alternative below (using operator *) but it generates temps

-			/////////////////////////////////////////////////////////////

-

-			// now set vhat_i to its true value by doing

-			// vhat_i += qidot * shat_i

-			spatJointVel.setZero();

-

-			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];

-

-			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint

-			spatVel[i + 1] += spatJointVel;

-

-			fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);

-			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);

-		}

-	}

-}

-

-void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const

-{

-	int num_links = getNumLinks();

-	///solve I * x = rhs, so the result = invI * rhs

-	if (num_links == 0)

-	{

-		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier

-

-		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))

-		{

-			result[0] = rhs_bot[0] / m_baseInertia[0];

-			result[1] = rhs_bot[1] / m_baseInertia[1];

-			result[2] = rhs_bot[2] / m_baseInertia[2];

-		}

-		else

-		{

-			result[0] = 0;

-			result[1] = 0;

-			result[2] = 0;

-		}

-		if (m_baseMass >= SIMD_EPSILON)

-		{

-			result[3] = rhs_top[0] / m_baseMass;

-			result[4] = rhs_top[1] / m_baseMass;

-			result[5] = rhs_top[2] / m_baseMass;

-		}

-		else

-		{

-			result[3] = 0;

-			result[4] = 0;

-			result[5] = 0;

-		}

-	}

-	else

-	{

-		if (!m_cachedInertiaValid)

-		{

-			for (int i = 0; i < 6; i++)

-			{

-				result[i] = 0.f;

-			}

-			return;

-		}

-		/// Special routine for calculating the inverse of a spatial inertia matrix

-		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices

-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;

-		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;

-		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();

-		tmp = invIupper_right * m_cachedInertiaLowerRight;

-		btMatrix3x3 invI_upper_left = (tmp * Binv);

-		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();

-		tmp = m_cachedInertiaTopLeft * invI_upper_left;

-		tmp[0][0] -= 1.0;

-		tmp[1][1] -= 1.0;

-		tmp[2][2] -= 1.0;

-		btMatrix3x3 invI_lower_left = (Binv * tmp);

-

-		//multiply result = invI * rhs

-		{

-			btVector3 vtop = invI_upper_left * rhs_top;

-			btVector3 tmp;

-			tmp = invIupper_right * rhs_bot;

-			vtop += tmp;

-			btVector3 vbot = invI_lower_left * rhs_top;

-			tmp = invI_lower_right * rhs_bot;

-			vbot += tmp;

-			result[0] = vtop[0];

-			result[1] = vtop[1];

-			result[2] = vtop[2];

-			result[3] = vbot[0];

-			result[4] = vbot[1];

-			result[5] = vbot[2];

-		}

-	}

-}

-void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const

-{

-	int num_links = getNumLinks();

-	///solve I * x = rhs, so the result = invI * rhs

-	if (num_links == 0)

-	{

-		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier

-		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))

-		{

-			result.setAngular(rhs.getAngular() / m_baseInertia);

-		}

-		else

-		{

-			result.setAngular(btVector3(0, 0, 0));

-		}

-		if (m_baseMass >= SIMD_EPSILON)

-		{

-			result.setLinear(rhs.getLinear() / m_baseMass);

-		}

-		else

-		{

-			result.setLinear(btVector3(0, 0, 0));

-		}

-	}

-	else

-	{

-		/// Special routine for calculating the inverse of a spatial inertia matrix

-		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices

-		if (!m_cachedInertiaValid)

-		{

-			result.setLinear(btVector3(0, 0, 0));

-			result.setAngular(btVector3(0, 0, 0));

-			result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));

-			return;

-		}

-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;

-		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;

-		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();

-		tmp = invIupper_right * m_cachedInertiaLowerRight;

-		btMatrix3x3 invI_upper_left = (tmp * Binv);

-		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();

-		tmp = m_cachedInertiaTopLeft * invI_upper_left;

-		tmp[0][0] -= 1.0;

-		tmp[1][1] -= 1.0;

-		tmp[2][2] -= 1.0;

-		btMatrix3x3 invI_lower_left = (Binv * tmp);

-

-		//multiply result = invI * rhs

-		{

-			btVector3 vtop = invI_upper_left * rhs.getLinear();

-			btVector3 tmp;

-			tmp = invIupper_right * rhs.getAngular();

-			vtop += tmp;

-			btVector3 vbot = invI_lower_left * rhs.getLinear();

-			tmp = invI_lower_right * rhs.getAngular();

-			vbot += tmp;

-			result.setVector(vtop, vbot);

-		}

-	}

-}

-

-void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const

-{

-	for (int row = 0; row < rowsA; row++)

-	{

-		for (int col = 0; col < colsB; col++)

-		{

-			pC[row * colsB + col] = 0.f;

-			for (int inner = 0; inner < rowsB; inner++)

-			{

-				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];

-			}

-		}

-	}

-}

-

-void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,

-												 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const

-{

-	// Temporary matrices/vectors -- use scratch space from caller

-	// so that we don't have to keep reallocating every frame

-

-	int num_links = getNumLinks();

-	scratch_r.resize(m_dofCount);

-	scratch_v.resize(4 * num_links + 4);

-

-	btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;

-	btVector3 *v_ptr = &scratch_v[0];

-

-	// zhat_i^A (scratch space)

-	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;

-	v_ptr += num_links * 2 + 2;

-

-	// rot_from_parent (cached from calcAccelerations)

-	const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];

-

-	// hhat (cached), accel (scratch)

-	// hhat is NOT stored for the base (but ahat is)

-	const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);

-	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;

-	v_ptr += num_links * 2 + 2;

-

-	// Y_i (scratch), invD_i (cached)

-	const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;

-	btScalar *Y = r_ptr;

-	////////////////

-	//aux variables

-	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies

-	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)

-	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough

-	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors

-	btSpatialTransformationMatrix fromParent;

-	/////////////////

-

-	// First 'upward' loop.

-	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

-

-	// Fill in zero_acc

-	// -- set to force/torque on the base, zero otherwise

-	if (m_fixedBase)

-	{

-		zeroAccSpatFrc[0].setZero();

-	}

-	else

-	{

-		//test forces

-		fromParent.m_rotMat = rot_from_parent[0];

-		fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);

-	}

-	for (int i = 0; i < num_links; ++i)

-	{

-		zeroAccSpatFrc[i + 1].setZero();

-	}

-

-	// 'Downward' loop.

-	// (part of TreeForwardDynamics in Mirtich.)

-	for (int i = num_links - 1; i >= 0; --i)

-	{

-		const int parent = m_links[i].m_parent;

-		fromParent.m_rotMat = rot_from_parent[i + 1];

-		fromParent.m_trnVec = m_links[i].m_cachedRVector;

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);

-		}

-

-		btVector3 in_top, in_bottom, out_top, out_bottom;

-		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			invD_times_Y[dof] = 0.f;

-

-			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)

-			{

-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];

-			}

-		}

-

-		// Zp += pXi * (Zi + hi*Yi/Di)

-		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];

-		}

-

-		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);

-

-		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];

-	}

-

-	// ptr to the joint accel part of the output

-	btScalar *joint_accel = output + 6;

-

-	// Second 'upward' loop

-	// (part of TreeForwardDynamics in Mirtich)

-

-	if (m_fixedBase)

-	{

-		spatAcc[0].setZero();

-	}

-	else

-	{

-		solveImatrix(zeroAccSpatFrc[0], result);

-		spatAcc[0] = -result;

-	}

-

-	// now do the loop over the m_links

-	for (int i = 0; i < num_links; ++i)

-	{

-		const int parent = m_links[i].m_parent;

-		fromParent.m_rotMat = rot_from_parent[i + 1];

-		fromParent.m_trnVec = m_links[i].m_cachedRVector;

-

-		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-		{

-			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];

-			//

-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);

-		}

-

-		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];

-		mulMatrix(const_cast<btScalar *>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);

-

-		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)

-			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];

-	}

-

-	// transform base accelerations back to the world frame.

-	btVector3 omegadot_out;

-	omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();

-	output[0] = omegadot_out[0];

-	output[1] = omegadot_out[1];

-	output[2] = omegadot_out[2];

-

-	btVector3 vdot_out;

-	vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();

-	output[3] = vdot_out[0];

-	output[4] = vdot_out[1];

-	output[5] = vdot_out[2];

-

-	/////////////////

-	//printf("delta = [");

-	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)

-	//	printf("%.2f ", output[dof]);

-	//printf("]\n");

-	/////////////////

-}

-

-void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)

-{

-	int num_links = getNumLinks();

-	// step position by adding dt * velocity

-	//btVector3 v = getBaseVel();

-	//m_basePos += dt * v;

-	//

-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);

-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)

-	//

-	pBasePos[0] += dt * pBaseVel[0];

-	pBasePos[1] += dt * pBaseVel[1];

-	pBasePos[2] += dt * pBaseVel[2];

-

-	///////////////////////////////

-	//local functor for quaternion integration (to avoid error prone redundancy)

-	struct

-	{

-		//"exponential map" based on btTransformUtil::integrateTransform(..)

-		void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)

-		{

-			//baseBody	=>	quat is alias and omega is global coor

-			//!baseBody	=>	quat is alibi and omega is local coor

-

-			btVector3 axis;

-			btVector3 angvel;

-

-			if (!baseBody)

-				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok

-			else

-				angvel = omega;

-

-			btScalar fAngle = angvel.length();

-			//limit the angular motion

-			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)

-			{

-				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;

-			}

-

-			if (fAngle < btScalar(0.001))

-			{

-				// use Taylor's expansions of sync function

-				axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);

-			}

-			else

-			{

-				// sync(fAngle) = sin(c*fAngle)/t

-				axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);

-			}

-

-			if (!baseBody)

-				quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;

-			else

-				quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));

-			//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();

-

-			quat.normalize();

-		}

-	} pQuatUpdateFun;

-	///////////////////////////////

-

-	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);

-	//

-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;

-	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)

-	//

-	btQuaternion baseQuat;

-	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);

-	btVector3 baseOmega;

-	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);

-	pQuatUpdateFun(baseOmega, baseQuat, true, dt);

-	pBaseQuat[0] = baseQuat.x();

-	pBaseQuat[1] = baseQuat.y();

-	pBaseQuat[2] = baseQuat.z();

-	pBaseQuat[3] = baseQuat.w();

-

-	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());

-	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());

-	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());

-

-	if (pq)

-		pq += 7;

-	if (pqd)

-		pqd += 6;

-

-	// Finally we can update m_jointPos for each of the m_links

-	for (int i = 0; i < num_links; ++i)

-	{

-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);

-		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));

-

-		switch (m_links[i].m_jointType)

-		{

-			case btMultibodyLink::ePrismatic:

-			case btMultibodyLink::eRevolute:

-			{

-				btScalar jointVel = pJointVel[0];

-				pJointPos[0] += dt * jointVel;

-				break;

-			}

-			case btMultibodyLink::eSpherical:

-			{

-				btVector3 jointVel;

-				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);

-				btQuaternion jointOri;

-				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);

-				pQuatUpdateFun(jointVel, jointOri, false, dt);

-				pJointPos[0] = jointOri.x();

-				pJointPos[1] = jointOri.y();

-				pJointPos[2] = jointOri.z();

-				pJointPos[3] = jointOri.w();

-				break;

-			}

-			case btMultibodyLink::ePlanar:

-			{

-				pJointPos[0] += dt * getJointVelMultiDof(i)[0];

-

-				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);

-				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);

-				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;

-				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;

-

-				break;

-			}

-			default:

-			{

-			}

-		}

-

-		m_links[i].updateCacheMultiDof(pq);

-

-		if (pq)

-			pq += m_links[i].m_posVarCount;

-		if (pqd)

-			pqd += m_links[i].m_dofCount;

-	}

-}

-

-void btMultiBody::fillConstraintJacobianMultiDof(int link,

-												 const btVector3 &contact_point,

-												 const btVector3 &normal_ang,

-												 const btVector3 &normal_lin,

-												 btScalar *jac,

-												 btAlignedObjectArray<btScalar> &scratch_r1,

-												 btAlignedObjectArray<btVector3> &scratch_v,

-												 btAlignedObjectArray<btMatrix3x3> &scratch_m) const

-{

-	// temporary space

-	int num_links = getNumLinks();

-	int m_dofCount = getNumDofs();

-	scratch_v.resize(3 * num_links + 3);  //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang

-	scratch_m.resize(num_links + 1);

-

-	btVector3 *v_ptr = &scratch_v[0];

-	btVector3 *p_minus_com_local = v_ptr;

-	v_ptr += num_links + 1;

-	btVector3 *n_local_lin = v_ptr;

-	v_ptr += num_links + 1;

-	btVector3 *n_local_ang = v_ptr;

-	v_ptr += num_links + 1;

-	btAssert(v_ptr - &scratch_v[0] == scratch_v.size());

-

-	//scratch_r.resize(m_dofCount);

-	//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;

-

-    scratch_r1.resize(m_dofCount+num_links);

-    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;

-    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;

-    int numLinksChildToRoot=0;

-    int l = link;

-    while (l != -1)

-    {

-        links[numLinksChildToRoot++]=l;

-        l = m_links[l].m_parent;

-    }

-    

-	btMatrix3x3 *rot_from_world = &scratch_m[0];

-

-	const btVector3 p_minus_com_world = contact_point - m_basePos;

-	const btVector3 &normal_lin_world = normal_lin;  //convenience

-	const btVector3 &normal_ang_world = normal_ang;

-

-	rot_from_world[0] = btMatrix3x3(m_baseQuat);

-

-	// omega coeffients first.

-	btVector3 omega_coeffs_world;

-	omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);

-	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];

-	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];

-	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];

-	// then v coefficients

-	jac[3] = normal_lin_world[0];

-	jac[4] = normal_lin_world[1];

-	jac[5] = normal_lin_world[2];

-

-	//create link-local versions of p_minus_com and normal

-	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;

-	n_local_lin[0] = rot_from_world[0] * normal_lin_world;

-	n_local_ang[0] = rot_from_world[0] * normal_ang_world;

-

-	// Set remaining jac values to zero for now.

-	for (int i = 6; i < 6 + m_dofCount; ++i)

-	{

-		jac[i] = 0;

-	}

-

-	// Qdot coefficients, if necessary.

-	if (num_links > 0 && link > -1)

-	{

-        // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,

-		// which is resulting in repeated work being done...)

-

-		// calculate required normals & positions in the local frames.

-        for (int a = 0; a < numLinksChildToRoot; a++)

-        {

-            int i = links[numLinksChildToRoot-1-a];

-        	// transform to local frame

-			const int parent = m_links[i].m_parent;

-			const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);

-			rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];

-

-			n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];

-			n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];

-			p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;

-

-			// calculate the jacobian entry

-			switch (m_links[i].m_jointType)

-			{

-				case btMultibodyLink::eRevolute:

-				{

-					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));

-					results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));

-					break;

-				}

-				case btMultibodyLink::ePrismatic:

-				{

-					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));

-					break;

-				}

-				case btMultibodyLink::eSpherical:

-				{

-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));

-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));

-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));

-

-					results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));

-					results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));

-					results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));

-

-					break;

-				}

-				case btMultibodyLink::ePlanar:

-				{

-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]));  // + m_links[i].getAxisBottom(0));

-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));

-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));

-

-					break;

-				}

-				default:

-				{

-				}

-			}

-		}

-

-		// Now copy through to output.

-		//printf("jac[%d] = ", link);

-		while (link != -1)

-		{

-			for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)

-			{

-				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];

-				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);

-			}

-

-			link = m_links[link].m_parent;

-		}

-		//printf("]\n");

-	}

-}

-

-void btMultiBody::wakeUp()

-{

-	m_sleepTimer = 0;

-	m_awake = true;

-}

-

-void btMultiBody::goToSleep()

-{

-	m_awake = false;

-}

-

-void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)

-{

-	extern bool gDisableDeactivation;

-	if (!m_canSleep || gDisableDeactivation)

-	{

-		m_awake = true;

-		m_sleepTimer = 0;

-		return;

-	}

-

-	

-

-	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)

-	btScalar motion = 0;

-	{

-		for (int i = 0; i < 6 + m_dofCount; ++i)

-			motion += m_realBuf[i] * m_realBuf[i];

-	}

-

-	if (motion < SLEEP_EPSILON)

-	{

-		m_sleepTimer += timestep;

-		if (m_sleepTimer > SLEEP_TIMEOUT)

-		{

-			goToSleep();

-		}

-	}

-	else

-	{

-		m_sleepTimer = 0;

-		if (m_canWakeup)

-		{

-			if (!m_awake)

-				wakeUp();

-		}

-	}

-}

-

-void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)

-{

-	int num_links = getNumLinks();

-

-	// Cached 3x3 rotation matrices from parent frame to this frame.

-	btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];

-

-	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?

-

-	for (int i = 0; i < num_links; ++i)

-	{

-		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);

-	}

-

-	int nLinks = getNumLinks();

-	///base + num m_links

-	world_to_local.resize(nLinks + 1);

-	local_origin.resize(nLinks + 1);

-

-	world_to_local[0] = getWorldToBaseRot();

-	local_origin[0] = getBasePos();

-

-	for (int k = 0; k < getNumLinks(); k++)

-	{

-		const int parent = getParent(k);

-		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];

-		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));

-	}

-

-	for (int link = 0; link < getNumLinks(); link++)

-	{

-		int index = link + 1;

-

-		btVector3 posr = local_origin[index];

-		btScalar 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]));

-		getLink(link).m_cachedWorldTransform = tr;

-	}

-}

-

-void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)

-{

-	world_to_local.resize(getNumLinks() + 1);

-	local_origin.resize(getNumLinks() + 1);

-

-	world_to_local[0] = getWorldToBaseRot();

-	local_origin[0] = getBasePos();

-

-	if (getBaseCollider())

-	{

-		btVector3 posr = local_origin[0];

-		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};

-		btScalar 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]));

-

-		getBaseCollider()->setWorldTransform(tr);

-	}

-

-	for (int k = 0; k < getNumLinks(); k++)

-	{

-		const int parent = getParent(k);

-		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];

-		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));

-	}

-

-	for (int m = 0; m < getNumLinks(); m++)

-	{

-		btMultiBodyLinkCollider *col = getLink(m).m_collider;

-		if (col)

-		{

-			int link = col->m_link;

-			btAssert(link == m);

-

-			int index = link + 1;

-

-			btVector3 posr = local_origin[index];

-			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};

-			btScalar 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]));

-

-			col->setWorldTransform(tr);

-		}

-	}

-}

-

-int btMultiBody::calculateSerializeBufferSize() const

-{

-	int sz = sizeof(btMultiBodyData);

-	return sz;

-}

-

-///fills the dataBuffer and returns the struct name (and 0 on failure)

-const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const

-{

-	btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;

-	getBasePos().serialize(mbd->m_baseWorldPosition);

-	getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);

-	getBaseVel().serialize(mbd->m_baseLinearVelocity);

-	getBaseOmega().serialize(mbd->m_baseAngularVelocity);

-

-	mbd->m_baseMass = this->getBaseMass();

-	getBaseInertia().serialize(mbd->m_baseInertia);

-	{

-		char *name = (char *)serializer->findNameForPointer(m_baseName);

-		mbd->m_baseName = (char *)serializer->getUniquePointer(name);

-		if (mbd->m_baseName)

-		{

-			serializer->serializeName(name);

-		}

-	}

-	mbd->m_numLinks = this->getNumLinks();

-	if (mbd->m_numLinks)

-	{

-		int sz = sizeof(btMultiBodyLinkData);

-		int numElem = mbd->m_numLinks;

-		btChunk *chunk = serializer->allocate(sz, numElem);

-		btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;

-		for (int i = 0; i < numElem; i++, memPtr++)

-		{

-			memPtr->m_jointType = getLink(i).m_jointType;

-			memPtr->m_dofCount = getLink(i).m_dofCount;

-			memPtr->m_posVarCount = getLink(i).m_posVarCount;

-

-			getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);

-

-			getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);

-			getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);

-			getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);

-			getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);

-

-			memPtr->m_linkMass = getLink(i).m_mass;

-			memPtr->m_parentIndex = getLink(i).m_parent;

-			memPtr->m_jointDamping = getLink(i).m_jointDamping;

-			memPtr->m_jointFriction = getLink(i).m_jointFriction;

-			memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;

-			memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;

-			memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;

-			memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;

-

-			getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);

-			getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);

-			getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);

-			btAssert(memPtr->m_dofCount <= 3);

-			for (int dof = 0; dof < getLink(i).m_dofCount; dof++)

-			{

-				getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);

-				getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);

-

-				memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];

-				memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];

-			}

-			int numPosVar = getLink(i).m_posVarCount;

-			for (int posvar = 0; posvar < numPosVar; posvar++)

-			{

-				memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];

-			}

-

-			{

-				char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);

-				memPtr->m_linkName = (char *)serializer->getUniquePointer(name);

-				if (memPtr->m_linkName)

-				{

-					serializer->serializeName(name);

-				}

-			}

-			{

-				char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);

-				memPtr->m_jointName = (char *)serializer->getUniquePointer(name);

-				if (memPtr->m_jointName)

-				{

-					serializer->serializeName(name);

-				}

-			}

-			memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);

-		}

-		serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);

-	}

-	mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;

-

-	// Fill padding with zeros to appease msan.

-#ifdef BT_USE_DOUBLE_PRECISION

-	memset(mbd->m_padding, 0, sizeof(mbd->m_padding));

-#endif

-

-	return btMultiBodyDataName;

-}

+/*
+ * PURPOSE:
+ *   Class representing an articulated rigid body. Stores the body's
+ *   current state, allows forces and torques to be set, handles
+ *   timestepping and implements Featherstone's algorithm.
+ *   
+ * COPYRIGHT:
+ *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
+ *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+ *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as 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 "btMultiBody.h"
+#include "btMultiBodyLink.h"
+#include "btMultiBodyLinkCollider.h"
+#include "btMultiBodyJointFeedback.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btSerializer.h"
+//#include "Bullet3Common/b3Logging.h"
+// #define INCLUDE_GYRO_TERM
+
+
+namespace
+{
+const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
+const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+}  // namespace
+
+void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+	const btVector3 &top_in,             // top part of input vector
+	const btVector3 &bottom_in,          // bottom part of input vector
+	btVector3 &top_out,                  // top part of output vector
+	btVector3 &bottom_out)               // bottom part of output vector
+{
+	top_out = rotation_matrix * top_in;
+	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
+}
+
+namespace
+{
+
+
+#if 0
+    void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
+                                 const btVector3 &displacement,
+                                 const btVector3 &top_in,
+                                 const btVector3 &bottom_in,
+                                 btVector3 &top_out,
+                                 btVector3 &bottom_out)
+    {
+        top_out = rotation_matrix.transpose() * top_in;
+        bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));		
+    }
+
+    btScalar SpatialDotProduct(const btVector3 &a_top,
+                            const btVector3 &a_bottom,
+                            const btVector3 &b_top,
+                            const btVector3 &b_bottom)
+    {
+        return a_bottom.dot(b_top) + a_top.dot(b_bottom);
+    }
+
+	void SpatialCrossProduct(const btVector3 &a_top,
+                            const btVector3 &a_bottom,
+                            const btVector3 &b_top,
+                            const btVector3 &b_bottom,
+							btVector3 &top_out,
+							btVector3 &bottom_out)
+	{
+		top_out = a_top.cross(b_top);
+		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
+	}
+#endif
+
+}  // namespace
+
+//
+// Implementation of class btMultiBody
+//
+
+btMultiBody::btMultiBody(int n_links,
+						 btScalar mass,
+						 const btVector3 &inertia,
+						 bool fixedBase,
+						 bool canSleep,
+						 bool /*deprecatedUseMultiDof*/)
+	: m_baseCollider(0),
+	  m_baseName(0),
+	  m_basePos(0, 0, 0),
+	  m_baseQuat(0, 0, 0, 1),
+	  m_baseMass(mass),
+	  m_baseInertia(inertia),
+
+	  m_fixedBase(fixedBase),
+	  m_awake(true),
+	  m_canSleep(canSleep),
+	  m_canWakeup(true),
+	  m_sleepTimer(0),
+	  m_userObjectPointer(0),
+	  m_userIndex2(-1),
+	  m_userIndex(-1),
+	  m_companionId(-1),
+	  m_linearDamping(0.04f),
+	  m_angularDamping(0.04f),
+	  m_useGyroTerm(true),
+	  m_maxAppliedImpulse(1000.f),
+	  m_maxCoordinateVelocity(100.f),
+	  m_hasSelfCollision(true),
+	  __posUpdated(false),
+	  m_dofCount(0),
+	  m_posVarCnt(0),
+	  m_useRK4(false),
+	  m_useGlobalVelocities(false),
+	  m_internalNeedsJointFeedback(false)
+{
+	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaValid = false;
+
+	m_links.resize(n_links);
+	m_matrixBuf.resize(n_links + 1);
+
+	m_baseForce.setValue(0, 0, 0);
+	m_baseTorque.setValue(0, 0, 0);
+
+	clearConstraintForces();
+	clearForcesAndTorques();
+}
+
+btMultiBody::~btMultiBody()
+{
+}
+
+void btMultiBody::setupFixed(int i,
+							 btScalar mass,
+							 const btVector3 &inertia,
+							 int parent,
+							 const btQuaternion &rotParentToThis,
+							 const btVector3 &parentComToThisPivotOffset,
+							 const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
+{
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_jointType = btMultibodyLink::eFixed;
+	m_links[i].m_dofCount = 0;
+	m_links[i].m_posVarCount = 0;
+
+	m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+
+	m_links[i].updateCacheMultiDof();
+
+	updateLinksDofOffsets();
+}
+
+void btMultiBody::setupPrismatic(int i,
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &jointAxis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
+{
+	m_dofCount += 1;
+	m_posVarCnt += 1;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, jointAxis);
+	m_links[i].m_eVector = parentComToThisPivotOffset;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_cachedRotParentToThis = rotParentToThis;
+
+	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
+	m_links[i].m_dofCount = 1;
+	m_links[i].m_posVarCount = 1;
+	m_links[i].m_jointPos[0] = 0.f;
+	m_links[i].m_jointTorque[0] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
+
+	m_links[i].updateCacheMultiDof();
+
+	updateLinksDofOffsets();
+}
+
+void btMultiBody::setupRevolute(int i,
+								btScalar mass,
+								const btVector3 &inertia,
+								int parent,
+								const btQuaternion &rotParentToThis,
+								const btVector3 &jointAxis,
+								const btVector3 &parentComToThisPivotOffset,
+								const btVector3 &thisPivotToThisComOffset,
+								bool disableParentCollision)
+{
+	m_dofCount += 1;
+	m_posVarCnt += 1;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].setAxisTop(0, jointAxis);
+	m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_jointType = btMultibodyLink::eRevolute;
+	m_links[i].m_dofCount = 1;
+	m_links[i].m_posVarCount = 1;
+	m_links[i].m_jointPos[0] = 0.f;
+	m_links[i].m_jointTorque[0] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
+	m_links[i].updateCacheMultiDof();
+	//
+	updateLinksDofOffsets();
+}
+
+void btMultiBody::setupSpherical(int i,
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
+{
+	m_dofCount += 3;
+	m_posVarCnt += 4;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_jointType = btMultibodyLink::eSpherical;
+	m_links[i].m_dofCount = 3;
+	m_links[i].m_posVarCount = 4;
+	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
+	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
+	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
+	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
+	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
+	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
+	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
+	m_links[i].m_jointPos[3] = 1.f;
+	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
+	m_links[i].updateCacheMultiDof();
+	//
+	updateLinksDofOffsets();
+}
+
+void btMultiBody::setupPlanar(int i,
+							  btScalar mass,
+							  const btVector3 &inertia,
+							  int parent,
+							  const btQuaternion &rotParentToThis,
+							  const btVector3 &rotationAxis,
+							  const btVector3 &parentComToThisComOffset,
+							  bool disableParentCollision)
+{
+	m_dofCount += 3;
+	m_posVarCnt += 3;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_dVector.setZero();
+	m_links[i].m_eVector = parentComToThisComOffset;
+
+	//
+	btVector3 vecNonParallelToRotAxis(1, 0, 0);
+	if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
+		vecNonParallelToRotAxis.setValue(0, 1, 0);
+	//
+
+	m_links[i].m_jointType = btMultibodyLink::ePlanar;
+	m_links[i].m_dofCount = 3;
+	m_links[i].m_posVarCount = 3;
+	btVector3 n = rotationAxis.normalized();
+	m_links[i].setAxisTop(0, n[0], n[1], n[2]);
+	m_links[i].setAxisTop(1, 0, 0, 0);
+	m_links[i].setAxisTop(2, 0, 0, 0);
+	m_links[i].setAxisBottom(0, 0, 0, 0);
+	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
+	m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
+	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
+	m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
+	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
+	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
+	m_links[i].updateCacheMultiDof();
+	//
+	updateLinksDofOffsets();
+
+	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
+	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
+}
+
+void btMultiBody::finalizeMultiDof()
+{
+	m_deltaV.resize(0);
+	m_deltaV.resize(6 + m_dofCount);
+	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
+	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+	m_matrixBuf.resize(m_links.size() + 1);
+	for (int i = 0; i < m_vectorBuf.size(); i++)
+	{
+		m_vectorBuf[i].setValue(0, 0, 0);
+	}
+	updateLinksDofOffsets();
+}
+
+int btMultiBody::getParent(int i) const
+{
+	return m_links[i].m_parent;
+}
+
+btScalar btMultiBody::getLinkMass(int i) const
+{
+	return m_links[i].m_mass;
+}
+
+const btVector3 &btMultiBody::getLinkInertia(int i) const
+{
+	return m_links[i].m_inertiaLocal;
+}
+
+btScalar btMultiBody::getJointPos(int i) const
+{
+	return m_links[i].m_jointPos[0];
+}
+
+btScalar btMultiBody::getJointVel(int i) const
+{
+	return m_realBuf[6 + m_links[i].m_dofOffset];
+}
+
+btScalar *btMultiBody::getJointPosMultiDof(int i)
+{
+	return &m_links[i].m_jointPos[0];
+}
+
+btScalar *btMultiBody::getJointVelMultiDof(int i)
+{
+	return &m_realBuf[6 + m_links[i].m_dofOffset];
+}
+
+const btScalar *btMultiBody::getJointPosMultiDof(int i) const
+{
+	return &m_links[i].m_jointPos[0];
+}
+
+const btScalar *btMultiBody::getJointVelMultiDof(int i) const
+{
+	return &m_realBuf[6 + m_links[i].m_dofOffset];
+}
+
+void btMultiBody::setJointPos(int i, btScalar q)
+{
+	m_links[i].m_jointPos[0] = q;
+	m_links[i].updateCacheMultiDof();
+}
+
+
+void btMultiBody::setJointPosMultiDof(int i, const double *q)
+{
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
+
+	m_links[i].updateCacheMultiDof();
+}
+
+void btMultiBody::setJointPosMultiDof(int i, const float *q)
+{
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
+
+	m_links[i].updateCacheMultiDof();
+}
+
+
+
+void btMultiBody::setJointVel(int i, btScalar qdot)
+{
+	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
+}
+
+void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
+}
+
+void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
+}
+
+const btVector3 &btMultiBody::getRVector(int i) const
+{
+	return m_links[i].m_cachedRVector;
+}
+
+const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
+{
+	return m_links[i].m_cachedRotParentToThis;
+}
+
+btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
+{
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_pos;
+	while (i != -1)
+	{
+		// 'result' is in frame i. transform it to frame parent(i)
+		result += getRVector(i);
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
+
+	// 'result' is now in the base frame. transform it to world frame
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	result += getBasePos();
+
+	return result;
+}
+
+btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
+{
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		// world to base
+		return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
+	}
+	else
+	{
+		// find position in parent frame, then transform to current frame
+		return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
+	}
+}
+
+btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
+{
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_dir;
+	while (i != -1)
+	{
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	return result;
+}
+
+btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
+{
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		return quatRotate(getWorldToBaseRot(), world_dir);
+	}
+	else
+	{
+		return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
+	}
+}
+
+btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
+{
+	btMatrix3x3 result = local_frame;
+	btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
+	btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
+	btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
+	result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
+	return result;
+}
+
+void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
+{
+	int num_links = getNumLinks();
+	// Calculates the velocities of each link (and the base) in its local frame
+	const btQuaternion& base_rot = getWorldToBaseRot();
+	omega[0] = quatRotate(base_rot, getBaseOmega());
+	vel[0] = quatRotate(base_rot, getBaseVel());
+
+	for (int i = 0; i < num_links; ++i)
+	{
+		const btMultibodyLink& link = getLink(i);
+		const int parent = link.m_parent;
+
+		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
+		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
+			omega[parent + 1], vel[parent + 1],
+			omega[i + 1], vel[i + 1]);
+
+		// now add qidot * shat_i
+		const btScalar* jointVel = getJointVelMultiDof(i);
+		for (int dof = 0; dof < link.m_dofCount; ++dof)
+		{
+			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
+			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
+		}
+	}
+}
+
+btScalar btMultiBody::getKineticEnergy() const
+{
+	int num_links = getNumLinks();
+	// TODO: would be better not to allocate memory here
+	btAlignedObjectArray<btVector3> omega;
+	omega.resize(num_links + 1);
+	btAlignedObjectArray<btVector3> vel;
+	vel.resize(num_links + 1);
+	compTreeLinkVelocities(&omega[0], &vel[0]);
+
+	// we will do the factor of 0.5 at the end
+	btScalar result = m_baseMass * vel[0].dot(vel[0]);
+	result += omega[0].dot(m_baseInertia * omega[0]);
+
+	for (int i = 0; i < num_links; ++i)
+	{
+		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
+		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
+	}
+
+	return 0.5f * result;
+}
+
+btVector3 btMultiBody::getAngularMomentum() const
+{
+	int num_links = getNumLinks();
+	// TODO: would be better not to allocate memory here
+	btAlignedObjectArray<btVector3> omega;
+	omega.resize(num_links + 1);
+	btAlignedObjectArray<btVector3> vel;
+	vel.resize(num_links + 1);
+	btAlignedObjectArray<btQuaternion> rot_from_world;
+	rot_from_world.resize(num_links + 1);
+	compTreeLinkVelocities(&omega[0], &vel[0]);
+
+	rot_from_world[0] = m_baseQuat;
+	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
+
+	for (int i = 0; i < num_links; ++i)
+	{
+		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
+		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
+	}
+
+	return result;
+}
+
+void btMultiBody::clearConstraintForces()
+{
+	m_baseConstraintForce.setValue(0, 0, 0);
+	m_baseConstraintTorque.setValue(0, 0, 0);
+
+	for (int i = 0; i < getNumLinks(); ++i)
+	{
+		m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
+		m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
+	}
+}
+void btMultiBody::clearForcesAndTorques()
+{
+	m_baseForce.setValue(0, 0, 0);
+	m_baseTorque.setValue(0, 0, 0);
+
+	for (int i = 0; i < getNumLinks(); ++i)
+	{
+		m_links[i].m_appliedForce.setValue(0, 0, 0);
+		m_links[i].m_appliedTorque.setValue(0, 0, 0);
+		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
+	}
+}
+
+void btMultiBody::clearVelocities()
+{
+	for (int i = 0; i < 6 + getNumDofs(); ++i)
+	{
+		m_realBuf[i] = 0.f;
+	}
+}
+void btMultiBody::addLinkForce(int i, const btVector3 &f)
+{
+	m_links[i].m_appliedForce += f;
+}
+
+void btMultiBody::addLinkTorque(int i, const btVector3 &t)
+{
+	m_links[i].m_appliedTorque += t;
+}
+
+void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
+{
+	m_links[i].m_appliedConstraintForce += f;
+}
+
+void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
+{
+	m_links[i].m_appliedConstraintTorque += t;
+}
+
+void btMultiBody::addJointTorque(int i, btScalar Q)
+{
+	m_links[i].m_jointTorque[0] += Q;
+}
+
+void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
+{
+	m_links[i].m_jointTorque[dof] += Q;
+}
+
+void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_links[i].m_jointTorque[dof] = Q[dof];
+}
+
+const btVector3 &btMultiBody::getLinkForce(int i) const
+{
+	return m_links[i].m_appliedForce;
+}
+
+const btVector3 &btMultiBody::getLinkTorque(int i) const
+{
+	return m_links[i].m_appliedTorque;
+}
+
+btScalar btMultiBody::getJointTorque(int i) const
+{
+	return m_links[i].m_jointTorque[0];
+}
+
+btScalar *btMultiBody::getJointTorqueMultiDof(int i)
+{
+	return &m_links[i].m_jointTorque[0];
+}
+
+inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
+{
+	btVector3 row0 = btVector3(
+		v0.x() * v1.x(),
+		v0.x() * v1.y(),
+		v0.x() * v1.z());
+	btVector3 row1 = btVector3(
+		v0.y() * v1.x(),
+		v0.y() * v1.y(),
+		v0.y() * v1.z());
+	btVector3 row2 = btVector3(
+		v0.z() * v1.x(),
+		v0.z() * v1.y(),
+		v0.z() * v1.z());
+
+	btMatrix3x3 m(row0[0], row0[1], row0[2],
+				  row1[0], row1[1], row1[2],
+				  row2[0], row2[1], row2[2]);
+	return m;
+}
+
+#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
+//
+
+void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
+    btAlignedObjectArray<btScalar> &scratch_r,
+    btAlignedObjectArray<btVector3> &scratch_v,
+    btAlignedObjectArray<btMatrix3x3> &scratch_m,
+	bool isConstraintPass,
+	bool jointFeedbackInWorldSpace,
+	bool jointFeedbackInJointFrame)
+{
+	// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
+	// and the base linear & angular accelerations.
+
+	// We apply damping forces in this routine as well as any external forces specified by the
+	// caller (via addBaseForce etc).
+
+	// output should point to an array of 6 + num_links reals.
+	// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
+	// num_links joint acceleration values.
+
+	// We added support for multi degree of freedom (multi dof) joints.
+	// In addition we also can compute the joint reaction forces. This is performed in a second pass,
+	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
+
+	m_internalNeedsJointFeedback = false;
+
+	int num_links = getNumLinks();
+
+	const btScalar DAMPING_K1_LINEAR = m_linearDamping;
+	const btScalar DAMPING_K2_LINEAR = m_linearDamping;
+
+	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
+	const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
+
+	const btVector3 base_vel = getBaseVel();
+	const btVector3 base_omega = getBaseOmega();
+
+	// Temporary matrices/vectors -- use scratch space from caller
+	// so that we don't have to keep reallocating every frame
+
+	scratch_r.resize(2 * m_dofCount + 7);  //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+	scratch_v.resize(8 * num_links + 6);
+	scratch_m.resize(4 * num_links + 4);
+
+	//btScalar * r_ptr = &scratch_r[0];
+	btScalar *output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
+	btVector3 *v_ptr = &scratch_v[0];
+
+	// vhat_i  (top = angular, bottom = linear part)
+	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+	// zhat_i^A
+	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+	// chat_i  (note NOT defined for the base)
+	btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2;
+	//
+	// Ihat_i^A.
+	btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
+
+	// Cached 3x3 rotation matrices from parent frame to this frame.
+	btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
+	btMatrix3x3 *rot_from_world = &scratch_m[0];
+
+	// hhat_i, ahat_i
+	// hhat is NOT stored for the base (but ahat is)
+	btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+	//
+	// Y_i, invD_i
+	btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar *Y = &scratch_r[0];
+	//
+	//aux variables
+	btSpatialMotionVector spatJointVel;         //spatial velocity due to the joint motion (i.e. without predecessors' influence)
+	btScalar D[36];                             //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;   //spatial transform from parent to child
+	btSymmetricSpatialDyad dyadTemp;            //inertia matrix temp
+	btSpatialTransformationMatrix fromWorld;
+	fromWorld.m_trnVec.setZero();
+	/////////////////
+
+	// ptr to the joint accel part of the output
+	btScalar *joint_accel = output + 6;
+
+	// Start of the algorithm proper.
+
+	// First 'upward' loop.
+	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?
+
+	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
+	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
+
+	if (m_fixedBase)
+	{
+		zeroAccSpatFrc[0].setZero();
+	}
+	else
+	{
+		const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
+		const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
+		//external forces
+		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
+
+		//adding damping terms (only)
+		const btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
+									linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
+
+		//
+		//p += vhat x Ihat vhat - done in a simpler way
+		if (m_useGyroTerm)
+			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
+		//
+		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	}
+
+	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
+	spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
+							 //
+							 btMatrix3x3(m_baseMass, 0, 0,
+										 0, m_baseMass, 0,
+										 0, 0, m_baseMass),
+							 //
+							 btMatrix3x3(m_baseInertia[0], 0, 0,
+										 0, m_baseInertia[1], 0,
+										 0, 0, m_baseInertia[2]));
+
+	rot_from_world[0] = rot_from_parent[0];
+
+	//
+	for (int i = 0; i < num_links; ++i)
+	{
+		const int parent = m_links[i].m_parent;
+		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
+
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		fromWorld.m_rotMat = rot_from_world[i + 1];
+		fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
+
+		// now set vhat_i to its true value by doing
+		// vhat_i += qidot * shat_i
+		if (!m_useGlobalVelocities)
+		{
+			spatJointVel.setZero();
+
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i + 1] += spatJointVel;
+
+			//
+			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
+			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
+		}
+		else
+		{
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
+		}
+
+		// we can now calculate chat_i
+		spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
+
+		// calculate zhat_i^A
+		//
+		//external forces
+		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
+		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
+
+		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
+
+#if 0	
+		{
+
+			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
+			i+1,
+			zeroAccSpatFrc[i+1].m_topVec[0],
+			zeroAccSpatFrc[i+1].m_topVec[1],
+			zeroAccSpatFrc[i+1].m_topVec[2],
+
+			zeroAccSpatFrc[i+1].m_bottomVec[0],
+			zeroAccSpatFrc[i+1].m_bottomVec[1],
+			zeroAccSpatFrc[i+1].m_bottomVec[2]);
+		}
+#endif
+		//
+		//adding damping terms (only)
+		btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
+										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+
+		// calculate Ihat_i^A
+		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
+		spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
+									 //
+									 btMatrix3x3(m_links[i].m_mass, 0, 0,
+												 0, m_links[i].m_mass, 0,
+												 0, 0, m_links[i].m_mass),
+									 //
+									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
+												 0, m_links[i].m_inertiaLocal[1], 0,
+												 0, 0, m_links[i].m_inertiaLocal[2]));
+		//
+		//p += vhat x Ihat vhat - done in a simpler way
+		if (m_useGyroTerm)
+			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
+		//
+		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
+		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+		////clamp parent's omega
+		//btScalar parOmegaMod = temp.length();
+		//btScalar parOmegaModMax = 1000;
+		//if(parOmegaMod > parOmegaModMax)
+		//	temp *= parOmegaModMax / parOmegaMod;
+		//zeroAccSpatFrc[i+1].addLinear(temp);
+		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
+		//temp = spatCoriolisAcc[i].getLinear();
+		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
+
+		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
+		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
+		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
+	}
+
+	// 'Downward' loop.
+	// (part of TreeForwardDynamics in Mirtich.)
+	for (int i = num_links - 1; i >= 0; --i)
+	{
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
+			//
+			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+		}
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
+			}
+		}
+
+		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+		switch (m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				if (D[0] >= SIMD_EPSILON)
+				{
+					invDi[0] = 1.0f / D[0];
+				}
+				else
+				{
+					invDi[0] = 0;
+				}
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			case btMultibodyLink::ePlanar:
+			{
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 invD3x3(D3x3.inverse());
+
+				//unroll the loop?
+				for (int row = 0; row < 3; ++row)
+				{
+					for (int col = 0; col < 3; ++col)
+					{
+						invDi[row * 3 + col] = invD3x3[row][col];
+					}
+				}
+
+				break;
+			}
+			default:
+			{
+			}
+		}
+
+		//determine h*D^{-1}
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			spatForceVecTemps[dof].setZero();
+
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				//
+				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
+			}
+		}
+
+		dyadTemp = spatInertia[i + 1];
+
+		//determine (h*D^{-1}) * h^{T}
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
+		}
+
+		fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
+			}
+		}
+
+		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
+		}
+
+		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
+
+		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
+	}
+
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
+
+	if (m_fixedBase)
+	{
+		spatAcc[0].setZero();
+	}
+	else
+	{
+		if (num_links > 0)
+		{
+			m_cachedInertiaValid = true;
+			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
+			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
+			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
+			m_cachedInertiaLowerRight = spatInertia[0].m_topLeftMat.transpose();
+		}
+
+		solveImatrix(zeroAccSpatFrc[0], result);
+		spatAcc[0] = -result;
+	}
+
+	// now do the loop over the m_links
+	for (int i = 0; i < num_links; ++i)
+	{
+		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
+		//	a = apar + cor + Sqdd
+		//or
+		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
+		//	a = apar + Sqdd
+
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
+		}
+
+		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+		//D^{-1} * (Y - h^{T}*apar)
+		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		spatAcc[i + 1] += spatCoriolisAcc[i];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+
+		if (m_links[i].m_jointFeedback)
+		{
+			m_internalNeedsJointFeedback = true;
+
+			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
+			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
+
+			if (jointFeedbackInJointFrame)
+			{
+				//shift the reaction forces to the joint frame
+				//linear (force) component is the same
+				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
+				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
+			}
+
+			if (jointFeedbackInWorldSpace)
+			{
+				if (isConstraintPass)
+				{
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
+				}
+				else
+				{
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
+				}
+			}
+			else
+			{
+				if (isConstraintPass)
+				{
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
+				}
+				else
+				{
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
+				}
+			}
+		}
+	}
+
+	// transform base accelerations back to the world frame.
+	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	output[0] = omegadot_out[0];
+	output[1] = omegadot_out[1];
+	output[2] = omegadot_out[2];
+
+	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+
+	/////////////////
+	//printf("q = [");
+	//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
+	//for(int link = 0; link < getNumLinks(); ++link)
+	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
+	//printf("]\n");
+	////
+	//printf("qd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", m_realBuf[dof]);
+	//printf("]\n");
+	//printf("qdd = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.6f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+
+	// Final step: add the accelerations (times dt) to the velocities.
+
+	if (!isConstraintPass)
+	{
+		if (dt > 0.)
+			applyDeltaVeeMultiDof(output, dt);
+	}
+	/////
+	//btScalar angularThres = 1;
+	//btScalar maxAngVel = 0.;
+	//bool scaleDown = 1.;
+	//for(int link = 0; link < m_links.size(); ++link)
+	//{
+	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
+	//	{
+	//		maxAngVel = spatVel[link+1].getAngular().length();
+	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
+	//		break;
+	//	}
+	//}
+
+	//if(scaleDown != 1.)
+	//{
+	//	for(int link = 0; link < m_links.size(); ++link)
+	//	{
+	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
+	//		{
+	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+	//				getJointVelMultiDof(link)[dof] *= scaleDown;
+	//		}
+	//	}
+	//}
+	/////
+
+	/////////////////////
+	if (m_useGlobalVelocities)
+	{
+		for (int i = 0; i < num_links; ++i)
+		{
+			const int parent = m_links[i].m_parent;
+			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
+			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
+
+			fromParent.m_rotMat = rot_from_parent[i + 1];
+			fromParent.m_trnVec = m_links[i].m_cachedRVector;
+			fromWorld.m_rotMat = rot_from_world[i + 1];
+
+			// vhat_i = i_xhat_p(i) * vhat_p(i)
+			fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
+			//nice alternative below (using operator *) but it generates temps
+			/////////////////////////////////////////////////////////////
+
+			// now set vhat_i to its true value by doing
+			// vhat_i += qidot * shat_i
+			spatJointVel.setZero();
+
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
+
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i + 1] += spatJointVel;
+
+			fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
+			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
+		}
+	}
+}
+
+void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
+{
+	int num_links = getNumLinks();
+	///solve I * x = rhs, so the result = invI * rhs
+	if (num_links == 0)
+	{
+		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result[0] = rhs_bot[0] / m_baseInertia[0];
+			result[1] = rhs_bot[1] / m_baseInertia[1];
+			result[2] = rhs_bot[2] / m_baseInertia[2];
+		}
+		else
+		{
+			result[0] = 0;
+			result[1] = 0;
+			result[2] = 0;
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result[3] = rhs_top[0] / m_baseMass;
+			result[4] = rhs_top[1] / m_baseMass;
+			result[5] = rhs_top[2] / m_baseMass;
+		}
+		else
+		{
+			result[3] = 0;
+			result[4] = 0;
+			result[5] = 0;
+		}
+	}
+	else
+	{
+		if (!m_cachedInertiaValid)
+		{
+			for (int i = 0; i < 6; i++)
+			{
+				result[i] = 0.f;
+			}
+			return;
+		}
+		/// Special routine for calculating the inverse of a spatial inertia matrix
+		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
+		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
+		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
+		tmp = invIupper_right * m_cachedInertiaLowerRight;
+		btMatrix3x3 invI_upper_left = (tmp * Binv);
+		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+		tmp = m_cachedInertiaTopLeft * invI_upper_left;
+		tmp[0][0] -= 1.0;
+		tmp[1][1] -= 1.0;
+		tmp[2][2] -= 1.0;
+		btMatrix3x3 invI_lower_left = (Binv * tmp);
+
+		//multiply result = invI * rhs
+		{
+			btVector3 vtop = invI_upper_left * rhs_top;
+			btVector3 tmp;
+			tmp = invIupper_right * rhs_bot;
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs_top;
+			tmp = invI_lower_right * rhs_bot;
+			vbot += tmp;
+			result[0] = vtop[0];
+			result[1] = vtop[1];
+			result[2] = vtop[2];
+			result[3] = vbot[0];
+			result[4] = vbot[1];
+			result[5] = vbot[2];
+		}
+	}
+}
+void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
+{
+	int num_links = getNumLinks();
+	///solve I * x = rhs, so the result = invI * rhs
+	if (num_links == 0)
+	{
+		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result.setAngular(rhs.getAngular() / m_baseInertia);
+		}
+		else
+		{
+			result.setAngular(btVector3(0, 0, 0));
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result.setLinear(rhs.getLinear() / m_baseMass);
+		}
+		else
+		{
+			result.setLinear(btVector3(0, 0, 0));
+		}
+	}
+	else
+	{
+		/// Special routine for calculating the inverse of a spatial inertia matrix
+		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+		if (!m_cachedInertiaValid)
+		{
+			result.setLinear(btVector3(0, 0, 0));
+			result.setAngular(btVector3(0, 0, 0));
+			result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
+			return;
+		}
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
+		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
+		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
+		tmp = invIupper_right * m_cachedInertiaLowerRight;
+		btMatrix3x3 invI_upper_left = (tmp * Binv);
+		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+		tmp = m_cachedInertiaTopLeft * invI_upper_left;
+		tmp[0][0] -= 1.0;
+		tmp[1][1] -= 1.0;
+		tmp[2][2] -= 1.0;
+		btMatrix3x3 invI_lower_left = (Binv * tmp);
+
+		//multiply result = invI * rhs
+		{
+			btVector3 vtop = invI_upper_left * rhs.getLinear();
+			btVector3 tmp;
+			tmp = invIupper_right * rhs.getAngular();
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs.getLinear();
+			tmp = invI_lower_right * rhs.getAngular();
+			vbot += tmp;
+			result.setVector(vtop, vbot);
+		}
+	}
+}
+
+void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
+{
+	for (int row = 0; row < rowsA; row++)
+	{
+		for (int col = 0; col < colsB; col++)
+		{
+			pC[row * colsB + col] = 0.f;
+			for (int inner = 0; inner < rowsB; inner++)
+			{
+				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
+			}
+		}
+	}
+}
+
+void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+												 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+{
+	// Temporary matrices/vectors -- use scratch space from caller
+	// so that we don't have to keep reallocating every frame
+
+	int num_links = getNumLinks();
+	scratch_r.resize(m_dofCount);
+	scratch_v.resize(4 * num_links + 4);
+
+	btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
+	btVector3 *v_ptr = &scratch_v[0];
+
+	// zhat_i^A (scratch space)
+	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+
+	// rot_from_parent (cached from calcAccelerations)
+	const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
+
+	// hhat (cached), accel (scratch)
+	// hhat is NOT stored for the base (but ahat is)
+	const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
+	v_ptr += num_links * 2 + 2;
+
+	// Y_i (scratch), invD_i (cached)
+	const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar *Y = r_ptr;
+	////////////////
+	//aux variables
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;
+	/////////////////
+
+	// First 'upward' loop.
+	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+	// Fill in zero_acc
+	// -- set to force/torque on the base, zero otherwise
+	if (m_fixedBase)
+	{
+		zeroAccSpatFrc[0].setZero();
+	}
+	else
+	{
+		//test forces
+		fromParent.m_rotMat = rot_from_parent[0];
+		fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);
+	}
+	for (int i = 0; i < num_links; ++i)
+	{
+		zeroAccSpatFrc[i + 1].setZero();
+	}
+
+	// 'Downward' loop.
+	// (part of TreeForwardDynamics in Mirtich.)
+	for (int i = num_links - 1; i >= 0; --i)
+	{
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);
+		}
+
+		btVector3 in_top, in_bottom, out_top, out_bottom;
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			invD_times_Y[dof] = 0.f;
+
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
+			}
+		}
+
+		// Zp += pXi * (Zi + hi*Yi/Di)
+		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
+		}
+
+		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
+
+		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
+	}
+
+	// ptr to the joint accel part of the output
+	btScalar *joint_accel = output + 6;
+
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
+
+	if (m_fixedBase)
+	{
+		spatAcc[0].setZero();
+	}
+	else
+	{
+		solveImatrix(zeroAccSpatFrc[0], result);
+		spatAcc[0] = -result;
+	}
+
+	// now do the loop over the m_links
+	for (int i = 0; i < num_links; ++i)
+	{
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
+		}
+
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+		mulMatrix(const_cast<btScalar *>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+	}
+
+	// transform base accelerations back to the world frame.
+	btVector3 omegadot_out;
+	omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	output[0] = omegadot_out[0];
+	output[1] = omegadot_out[1];
+	output[2] = omegadot_out[2];
+
+	btVector3 vdot_out;
+	vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
+	output[3] = vdot_out[0];
+	output[4] = vdot_out[1];
+	output[5] = vdot_out[2];
+
+	/////////////////
+	//printf("delta = [");
+	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
+	//	printf("%.2f ", output[dof]);
+	//printf("]\n");
+	/////////////////
+}
+
+void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
+{
+	int num_links = getNumLinks();
+	// step position by adding dt * velocity
+	//btVector3 v = getBaseVel();
+	//m_basePos += dt * v;
+	//
+	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+	//
+	pBasePos[0] += dt * pBaseVel[0];
+	pBasePos[1] += dt * pBaseVel[1];
+	pBasePos[2] += dt * pBaseVel[2];
+
+	///////////////////////////////
+	//local functor for quaternion integration (to avoid error prone redundancy)
+	struct
+	{
+		//"exponential map" based on btTransformUtil::integrateTransform(..)
+		void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+		{
+			//baseBody	=>	quat is alias and omega is global coor
+			//!baseBody	=>	quat is alibi and omega is local coor
+
+			btVector3 axis;
+			btVector3 angvel;
+
+			if (!baseBody)
+				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
+			else
+				angvel = omega;
+
+			btScalar fAngle = angvel.length();
+			//limit the angular motion
+			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
+			}
+
+			if (fAngle < btScalar(0.001))
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
+			}
+
+			if (!baseBody)
+				quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+			else
+				quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+			//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+
+			quat.normalize();
+		}
+	} pQuatUpdateFun;
+	///////////////////////////////
+
+	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+	//
+	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+	//
+	btQuaternion baseQuat;
+	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+	btVector3 baseOmega;
+	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+	pBaseQuat[0] = baseQuat.x();
+	pBaseQuat[1] = baseQuat.y();
+	pBaseQuat[2] = baseQuat.z();
+	pBaseQuat[3] = baseQuat.w();
+
+	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
+	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
+	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
+
+	if (pq)
+		pq += 7;
+	if (pqd)
+		pqd += 6;
+
+	// Finally we can update m_jointPos for each of the m_links
+	for (int i = 0; i < num_links; ++i)
+	{
+		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
+		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
+
+		switch (m_links[i].m_jointType)
+		{
+			case btMultibodyLink::ePrismatic:
+			case btMultibodyLink::eRevolute:
+			{
+				btScalar jointVel = pJointVel[0];
+				pJointPos[0] += dt * jointVel;
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			{
+				btVector3 jointVel;
+				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+				btQuaternion jointOri;
+				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+				pQuatUpdateFun(jointVel, jointOri, false, dt);
+				pJointPos[0] = jointOri.x();
+				pJointPos[1] = jointOri.y();
+				pJointPos[2] = jointOri.z();
+				pJointPos[3] = jointOri.w();
+				break;
+			}
+			case btMultibodyLink::ePlanar:
+			{
+				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+
+				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+
+				break;
+			}
+			default:
+			{
+			}
+		}
+
+		m_links[i].updateCacheMultiDof(pq);
+
+		if (pq)
+			pq += m_links[i].m_posVarCount;
+		if (pqd)
+			pqd += m_links[i].m_dofCount;
+	}
+}
+
+void btMultiBody::fillConstraintJacobianMultiDof(int link,
+												 const btVector3 &contact_point,
+												 const btVector3 &normal_ang,
+												 const btVector3 &normal_lin,
+												 btScalar *jac,
+												 btAlignedObjectArray<btScalar> &scratch_r1,
+												 btAlignedObjectArray<btVector3> &scratch_v,
+												 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
+{
+	// temporary space
+	int num_links = getNumLinks();
+	int m_dofCount = getNumDofs();
+	scratch_v.resize(3 * num_links + 3);  //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
+	scratch_m.resize(num_links + 1);
+
+	btVector3 *v_ptr = &scratch_v[0];
+	btVector3 *p_minus_com_local = v_ptr;
+	v_ptr += num_links + 1;
+	btVector3 *n_local_lin = v_ptr;
+	v_ptr += num_links + 1;
+	btVector3 *n_local_ang = v_ptr;
+	v_ptr += num_links + 1;
+	btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+	//scratch_r.resize(m_dofCount);
+	//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
+
+    scratch_r1.resize(m_dofCount+num_links);
+    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
+    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
+    int numLinksChildToRoot=0;
+    int l = link;
+    while (l != -1)
+    {
+        links[numLinksChildToRoot++]=l;
+        l = m_links[l].m_parent;
+    }
+    
+	btMatrix3x3 *rot_from_world = &scratch_m[0];
+
+	const btVector3 p_minus_com_world = contact_point - m_basePos;
+	const btVector3 &normal_lin_world = normal_lin;  //convenience
+	const btVector3 &normal_ang_world = normal_ang;
+
+	rot_from_world[0] = btMatrix3x3(m_baseQuat);
+
+	// omega coeffients first.
+	btVector3 omega_coeffs_world;
+	omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
+	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
+	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
+	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
+	// then v coefficients
+	jac[3] = normal_lin_world[0];
+	jac[4] = normal_lin_world[1];
+	jac[5] = normal_lin_world[2];
+
+	//create link-local versions of p_minus_com and normal
+	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
+	n_local_lin[0] = rot_from_world[0] * normal_lin_world;
+	n_local_ang[0] = rot_from_world[0] * normal_ang_world;
+
+	// Set remaining jac values to zero for now.
+	for (int i = 6; i < 6 + m_dofCount; ++i)
+	{
+		jac[i] = 0;
+	}
+
+	// Qdot coefficients, if necessary.
+	if (num_links > 0 && link > -1)
+	{
+        // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+		// which is resulting in repeated work being done...)
+
+		// calculate required normals & positions in the local frames.
+        for (int a = 0; a < numLinksChildToRoot; a++)
+        {
+            int i = links[numLinksChildToRoot-1-a];
+        	// transform to local frame
+			const int parent = m_links[i].m_parent;
+			const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
+			rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
+
+			n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
+			n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
+			p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
+
+			// calculate the jacobian entry
+			switch (m_links[i].m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
+					break;
+				}
+				case btMultibodyLink::eSpherical:
+				{
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
+
+					results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
+					results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
+					results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
+
+					break;
+				}
+				case btMultibodyLink::ePlanar:
+				{
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]));  // + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
+
+					break;
+				}
+				default:
+				{
+				}
+			}
+		}
+
+		// Now copy through to output.
+		//printf("jac[%d] = ", link);
+		while (link != -1)
+		{
+			for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+			{
+				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
+				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
+			}
+
+			link = m_links[link].m_parent;
+		}
+		//printf("]\n");
+	}
+}
+
+void btMultiBody::wakeUp()
+{
+	m_sleepTimer = 0;
+	m_awake = true;
+}
+
+void btMultiBody::goToSleep()
+{
+	m_awake = false;
+}
+
+void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
+{
+	extern bool gDisableDeactivation;
+	if (!m_canSleep || gDisableDeactivation)
+	{
+		m_awake = true;
+		m_sleepTimer = 0;
+		return;
+	}
+
+	
+
+	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
+	btScalar motion = 0;
+	{
+		for (int i = 0; i < 6 + m_dofCount; ++i)
+			motion += m_realBuf[i] * m_realBuf[i];
+	}
+
+	if (motion < SLEEP_EPSILON)
+	{
+		m_sleepTimer += timestep;
+		if (m_sleepTimer > SLEEP_TIMEOUT)
+		{
+			goToSleep();
+		}
+	}
+	else
+	{
+		m_sleepTimer = 0;
+		if (m_canWakeup)
+		{
+			if (!m_awake)
+				wakeUp();
+		}
+	}
+}
+
+void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+	int num_links = getNumLinks();
+
+	// Cached 3x3 rotation matrices from parent frame to this frame.
+	btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
+
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?
+
+	for (int i = 0; i < num_links; ++i)
+	{
+		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+	}
+
+	int nLinks = getNumLinks();
+	///base + num m_links
+	world_to_local.resize(nLinks + 1);
+	local_origin.resize(nLinks + 1);
+
+	world_to_local[0] = getWorldToBaseRot();
+	local_origin[0] = getBasePos();
+
+	for (int k = 0; k < getNumLinks(); k++)
+	{
+		const int parent = getParent(k);
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
+	}
+
+	for (int link = 0; link < getNumLinks(); link++)
+	{
+		int index = link + 1;
+
+		btVector3 posr = local_origin[index];
+		btScalar 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]));
+		getLink(link).m_cachedWorldTransform = tr;
+	}
+}
+
+void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+	world_to_local.resize(getNumLinks() + 1);
+	local_origin.resize(getNumLinks() + 1);
+
+	world_to_local[0] = getWorldToBaseRot();
+	local_origin[0] = getBasePos();
+
+	if (getBaseCollider())
+	{
+		btVector3 posr = local_origin[0];
+		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+		btScalar 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]));
+
+		getBaseCollider()->setWorldTransform(tr);
+	}
+
+	for (int k = 0; k < getNumLinks(); k++)
+	{
+		const int parent = getParent(k);
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
+	}
+
+	for (int m = 0; m < getNumLinks(); m++)
+	{
+		btMultiBodyLinkCollider *col = getLink(m).m_collider;
+		if (col)
+		{
+			int link = col->m_link;
+			btAssert(link == m);
+
+			int index = link + 1;
+
+			btVector3 posr = local_origin[index];
+			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+			btScalar 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]));
+
+			col->setWorldTransform(tr);
+		}
+	}
+}
+
+int btMultiBody::calculateSerializeBufferSize() const
+{
+	int sz = sizeof(btMultiBodyData);
+	return sz;
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
+{
+	btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
+	getBasePos().serialize(mbd->m_baseWorldPosition);
+	getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+	getBaseVel().serialize(mbd->m_baseLinearVelocity);
+	getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
+	mbd->m_baseMass = this->getBaseMass();
+	getBaseInertia().serialize(mbd->m_baseInertia);
+	{
+		char *name = (char *)serializer->findNameForPointer(m_baseName);
+		mbd->m_baseName = (char *)serializer->getUniquePointer(name);
+		if (mbd->m_baseName)
+		{
+			serializer->serializeName(name);
+		}
+	}
+	mbd->m_numLinks = this->getNumLinks();
+	if (mbd->m_numLinks)
+	{
+		int sz = sizeof(btMultiBodyLinkData);
+		int numElem = mbd->m_numLinks;
+		btChunk *chunk = serializer->allocate(sz, numElem);
+		btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
+		for (int i = 0; i < numElem; i++, memPtr++)
+		{
+			memPtr->m_jointType = getLink(i).m_jointType;
+			memPtr->m_dofCount = getLink(i).m_dofCount;
+			memPtr->m_posVarCount = getLink(i).m_posVarCount;
+
+			getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+
+			getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
+			getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
+			getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
+			getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
+
+			memPtr->m_linkMass = getLink(i).m_mass;
+			memPtr->m_parentIndex = getLink(i).m_parent;
+			memPtr->m_jointDamping = getLink(i).m_jointDamping;
+			memPtr->m_jointFriction = getLink(i).m_jointFriction;
+			memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
+			memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
+			memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
+			memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
+
+			getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
+			getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
+			getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
+			btAssert(memPtr->m_dofCount <= 3);
+			for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
+			{
+				getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
+				getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
+
+				memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
+				memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+			}
+			int numPosVar = getLink(i).m_posVarCount;
+			for (int posvar = 0; posvar < numPosVar; posvar++)
+			{
+				memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+			}
+
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
+				memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_linkName)
+				{
+					serializer->serializeName(name);
+				}
+			}
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
+				memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_jointName)
+				{
+					serializer->serializeName(name);
+				}
+			}
+			memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
+		}
+		serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
+	}
+	mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
+
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
+#endif
+
+	return btMultiBodyDataName;
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 4aa083741..79fae9efd 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -1,843 +1,843 @@
-/*

- * PURPOSE:

- *   Class representing an articulated rigid body. Stores the body's

- *   current state, allows forces and torques to be set, handles

- *   timestepping and implements Featherstone's algorithm.

- *   

- * COPYRIGHT:

- *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013

- *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)

- *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements

-

- This software is provided 'as-is', without any express or implied warranty.

- In no event will the authors be held liable for any damages arising from the use of this software.

- Permission is granted to anyone to use this software for any purpose,

- including commercial applications, and to alter it and redistribute it freely,

- subject to the following restrictions:

- 

- 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

- 2. Altered source versions must be plainly marked as 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_MULTIBODY_H

-#define BT_MULTIBODY_H

-

-#include "LinearMath/btScalar.h"

-#include "LinearMath/btVector3.h"

-#include "LinearMath/btQuaternion.h"

-#include "LinearMath/btMatrix3x3.h"

-#include "LinearMath/btAlignedObjectArray.h"

-

-///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms

-#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;

-

-ATTRIBUTE_ALIGNED16(class)

-btMultiBody

-{

-public:

-	BT_DECLARE_ALIGNED_ALLOCATOR();

-

-	//

-	// 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 fixedBase,            // whether the base is fixed (true) or can move (false)

-				bool canSleep, bool deprecatedMultiDof = true);

-

-	virtual ~btMultiBody();

-

-	//note: fixed link collision with parent is always disabled

-	void setupFixed(int linkIndex,

-					btScalar mass,

-					const btVector3 &inertia,

-					int parent,

-					const btQuaternion &rotParentToThis,

-					const btVector3 &parentComToThisPivotOffset,

-					const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);

-

-	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 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 m_links[index];

-	}

-

-	btMultibodyLink &getLink(int index)

-	{

-		return m_links[index];

-	}

-

-	void setBaseCollider(btMultiBodyLinkCollider * collider)  //collider can be NULL to disable collision for the base

-	{

-		m_baseCollider = collider;

-	}

-	const btMultiBodyLinkCollider *getBaseCollider() const

-	{

-		return m_baseCollider;

-	}

-	btMultiBodyLinkCollider *getBaseCollider()

-	{

-		return m_baseCollider;

-	}

-

-	const btMultiBodyLinkCollider *getLinkCollider(int index) const

-	{

-		if (index >= 0 && index < getNumLinks())

-		{

-			return getLink(index).m_collider;

-		}

-		return 0;

-	}

-

-	btMultiBodyLinkCollider *getLinkCollider(int index)

-	{

-		if (index >= 0 && index < getNumLinks())

-		{

-			return getLink(index).m_collider;

-		}

-		return 0;

-	}

-

-	//

-	// get parent

-	// input: link num from 0 to num_links-1

-	// output: link num from 0 to num_links-1, OR -1 to mean the base.

-	//

-	int getParent(int link_num) const;

-

-	//

-	// get number of m_links, masses, moments of 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) { 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 m_basePos; 

-	}  // in world frame

-	const btVector3 getBaseVel() const

-	{

-		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);

-	}  // in world frame

-	const btQuaternion &getWorldToBaseRot() const

-	{

-		return m_baseQuat;

-	}                                                                                               // rotates world vectors into base frame

-	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame

-

-	void setBasePos(const btVector3 &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_realBuf[3] = vel[0];

-		m_realBuf[4] = vel[1];

-		m_realBuf[5] = vel[2];

-	}

-	void setWorldToBaseRot(const btQuaternion &rot)

-	{

-		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?

-	}

-	void setBaseOmega(const btVector3 &omega)

-	{

-		m_realBuf[0] = omega[0];

-		m_realBuf[1] = omega[1];

-		m_realBuf[2] = omega[2];

-	}

-

-	//

-	// 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, const double *q);

-	void setJointVelMultiDof(int i, const double *qdot);

-	void setJointPosMultiDof(int i, const float *q);

-	void setJointVelMultiDof(int i, const float *qdot);

-

-	//

-	// direct access to velocities as a vector of 6 + num_links elements.

-	// (omega first, then v, then joint velocities.)

-	//

-	const btScalar *getVelocityVector() const

-	{

-		return &m_realBuf[0];

-	}

-	/*    btScalar * getVelocityVector() 

-	{ 

-		return &real_buf[0]; 

-	}

-  */

-

-	//

-	// get the frames of reference (positions and orientations) of the child m_links

-	// (i = 0 to num_links-1)

-	//

-

-	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords

-	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.

-

-	//

-	// transform vectors in local frame of link i to world frame (or vice versa)

-	//

-	btVector3 localPosToWorld(int i, const btVector3 &vec) const;

-	btVector3 localDirToWorld(int i, const btVector3 &vec) const;

-	btVector3 worldPosToLocal(int i, const btVector3 &vec) const;

-	btVector3 worldDirToLocal(int i, const btVector3 &vec) const;

-

-	//

-	// transform a frame in local coordinate to a frame in world coordinate

-	//

-	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;

-

-	//

-	// calculate kinetic energy and angular momentum

-	// useful for debugging.

-	//

-

-	btScalar getKineticEnergy() const;

-	btVector3 getAngularMomentum() const;

-

-	//

-	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.

-	//

-

-	void clearForcesAndTorques();

-	void clearConstraintForces();

-

-	void clearVelocities();

-

-	void addBaseForce(const btVector3 &f)

-	{

-		m_baseForce += f;

-	}

-	void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }

-	void addLinkForce(int i, const btVector3 &f);

-	void addLinkTorque(int i, const btVector3 &t);

-

-	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);

-

-	//

-	// dynamics routines.

-	//

-

-	// timestep the velocities (given the external forces/torques set using addBaseForce etc).

-	// also sets up caches for calcAccelerationDeltas.

-	//

-	// Note: the caller must provide three vectors which are used as

-	// temporary scratch space. The idea here is to reduce dynamic

-	// memory allocation: the same scratch vectors can be re-used

-	// again and again for different Multibodies, instead of each

-	// btMultiBody allocating (and then deallocating) their own

-	// individual scratch buffers. This gives a considerable speed

-	// improvement, at least on Windows (where dynamic memory

-	// allocation appears to be fairly slow).

-	//

-

-	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,

-															  btAlignedObjectArray<btScalar> & scratch_r,

-															  btAlignedObjectArray<btVector3> & scratch_v,

-															  btAlignedObjectArray<btMatrix3x3> & scratch_m,

-															  bool isConstraintPass,

-                                                              bool jointFeedbackInWorldSpace,

-                                                              bool jointFeedbackInJointFrame

-                                                              );

-

-	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead

-	//void stepVelocitiesMultiDof(btScalar dt,

-	//							btAlignedObjectArray<btScalar> & scratch_r,

-	//							btAlignedObjectArray<btVector3> & scratch_v,

-	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,

-	//							bool isConstraintPass = false)

-	//{

-	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);

-	//}

-

-	// calcAccelerationDeltasMultiDof

-	// input: force vector (in same format as jacobian, i.e.:

-	//                      3 torque values, 3 force values, num_links joint torque values)

-	// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values

-	// (existing contents of output array are replaced)

-	// calcAccelerationDeltasMultiDof must have been called first.

-	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,

-										btAlignedObjectArray<btScalar> &scratch_r,

-										btAlignedObjectArray<btVector3> &scratch_v) const;

-

-	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 stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);

-

-	//

-	// contacts

-	//

-

-	// This routine fills out a contact constraint jacobian for this body.

-	// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.

-	// 'normal' & 'contact_point' are both given in world coordinates.

-

-	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 { fillConstraintJacobianMultiDof(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 fillConstraintJacobianMultiDof(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)

-	{

-		if (m_canWakeup)

-		{

-			m_canSleep = canSleep;

-		}

-	}

-

-	bool getCanSleep() const

-	{

-		return m_canSleep;

-	}

-

-	bool getCanWakeup() const

-	{

-		return m_canWakeup;

-	}

-	

-	void setCanWakeup(bool canWakeup) 

-	{

-		m_canWakeup = canWakeup;

-	}

-	bool isAwake() const { return m_awake; }

-	void wakeUp();

-	void goToSleep();

-	void checkMotionAndSleepIfRequired(btScalar timestep);

-

-	bool hasFixedBase() const

-	{

-		return m_fixedBase;

-	}

-

-	void setFixedBase(bool fixedBase)

-	{

-		m_fixedBase = fixedBase;

-	}

-

-	int getCompanionId() const

-	{

-		return m_companionId;

-	}

-	void setCompanionId(int id)

-	{

-		//printf("for %p setCompanionId(%d)\n",this, id);

-		m_companionId = id;

-	}

-

-	void setNumLinks(int numLinks)  //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links

-	{

-		m_links.resize(numLinks);

-	}

-

-	btScalar getLinearDamping() const

-	{

-		return m_linearDamping;

-	}

-	void setLinearDamping(btScalar damp)

-	{

-		m_linearDamping = damp;

-	}

-	btScalar getAngularDamping() const

-	{

-		return m_angularDamping;

-	}

-	void setAngularDamping(btScalar damp)

-	{

-		m_angularDamping = damp;

-	}

-

-	bool getUseGyroTerm() const

-	{

-		return m_useGyroTerm;

-	}

-	void setUseGyroTerm(bool useGyro)

-	{

-		m_useGyroTerm = useGyro;

-	}

-	btScalar getMaxCoordinateVelocity() const

-	{

-		return m_maxCoordinateVelocity;

-	}

-	void setMaxCoordinateVelocity(btScalar maxVel)

-	{

-		m_maxCoordinateVelocity = maxVel;

-	}

-

-	btScalar getMaxAppliedImpulse() const

-	{

-		return m_maxAppliedImpulse;

-	}

-	void setMaxAppliedImpulse(btScalar maxImp)

-	{

-		m_maxAppliedImpulse = maxImp;

-	}

-	void setHasSelfCollision(bool hasSelfCollision)

-	{

-		m_hasSelfCollision = hasSelfCollision;

-	}

-	bool hasSelfCollision() const

-	{

-		return m_hasSelfCollision;

-	}

-

-	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 compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;

-

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

-	}

-

-	///users can point to their objects, userPointer is not used by Bullet

-	void *getUserPointer() const

-	{

-		return m_userObjectPointer;

-	}

-

-	int getUserIndex() const

-	{

-		return m_userIndex;

-	}

-

-	int getUserIndex2() const

-	{

-		return m_userIndex2;

-	}

-	///users can point to their objects, userPointer is not used by Bullet

-	void setUserPointer(void *userPointer)

-	{

-		m_userObjectPointer = userPointer;

-	}

-

-	///users can point to their objects, userPointer is not used by Bullet

-	void setUserIndex(int index)

-	{

-		m_userIndex = index;

-	}

-

-	void setUserIndex2(int index)

-	{

-		m_userIndex2 = index;

-	}

-

-	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame

-		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates

-		const btVector3 &top_in,       // top part of input vector

-		const btVector3 &bottom_in,    // bottom part of input vector

-		btVector3 &top_out,         // top part of output vector

-		btVector3 &bottom_out);      // bottom part of output vector

-

-

-

-private:

-	btMultiBody(const btMultiBody &);     // not implemented

-	void operator=(const btMultiBody &);  // not implemented

-

-	void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar 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 m_basePos;      // position of COM of base (world frame)

-	btQuaternion m_baseQuat;  // rotates world points into base frame

-

-	btScalar m_baseMass;      // mass of the base

-	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)

-

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

-

-	//

-	// realBuf:

-	//  offset         size            array

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

-	//

-	// vectorBuf:

-	//  offset         size         array

-	//   0              num_links    h_top

-	//   num_links      num_links    h_bottom

-	//

-	// matrixBuf:

-	//  offset         size         array

-	//   0              num_links+1  rot_from_parent

-	//

-	btAlignedObjectArray<btScalar> m_deltaV;

-	btAlignedObjectArray<btScalar> m_realBuf;

-	btAlignedObjectArray<btVector3> m_vectorBuf;

-	btAlignedObjectArray<btMatrix3x3> m_matrixBuf;

-

-	btMatrix3x3 m_cachedInertiaTopLeft;

-	btMatrix3x3 m_cachedInertiaTopRight;

-	btMatrix3x3 m_cachedInertiaLowerLeft;

-	btMatrix3x3 m_cachedInertiaLowerRight;

-	bool m_cachedInertiaValid;

-

-	bool m_fixedBase;

-

-	// Sleep parameters.

-	bool m_awake;

-	bool m_canSleep;

-	bool m_canWakeup;

-	btScalar m_sleepTimer;

-

-	void *m_userObjectPointer;

-	int m_userIndex2;

-	int m_userIndex;

-

-	int m_companionId;

-	btScalar m_linearDamping;

-	btScalar m_angularDamping;

-	bool m_useGyroTerm;

-	btScalar m_maxAppliedImpulse;

-	btScalar m_maxCoordinateVelocity;

-	bool m_hasSelfCollision;

-

-	bool __posUpdated;

-	int m_dofCount, m_posVarCnt;

-

-	bool m_useRK4, m_useGlobalVelocities;

-	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis

-	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing

-

-	///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_parentComToThisPivotOffset;

-	btVector3DoubleData m_thisPivotToThisComOffset;

-	btVector3DoubleData m_jointAxisTop[6];

-	btVector3DoubleData m_jointAxisBottom[6];

-

-	btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)

-	btVector3DoubleData m_absFrameTotVelocityTop;

-	btVector3DoubleData m_absFrameTotVelocityBottom;

-	btVector3DoubleData m_absFrameLocVelocityTop;

-	btVector3DoubleData m_absFrameLocVelocityBottom;

-

-	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];

-

-	double m_jointDamping;

-	double m_jointFriction;

-	double m_jointLowerLimit;

-	double m_jointUpperLimit;

-	double m_jointMaxForce;

-	double m_jointMaxVelocity;

-

-	char *m_linkName;

-	char *m_jointName;

-	btCollisionObjectDoubleData *m_linkCollider;

-	char *m_paddingPtr;

-};

-

-struct btMultiBodyLinkFloatData

-{

-	btQuaternionFloatData m_zeroRotParentToThis;

-	btVector3FloatData m_parentComToThisPivotOffset;

-	btVector3FloatData m_thisPivotToThisComOffset;

-	btVector3FloatData m_jointAxisTop[6];

-	btVector3FloatData m_jointAxisBottom[6];

-	btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)

-	btVector3FloatData m_absFrameTotVelocityTop;

-	btVector3FloatData m_absFrameTotVelocityBottom;

-	btVector3FloatData m_absFrameLocVelocityTop;

-	btVector3FloatData m_absFrameLocVelocityBottom;

-

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

-	float m_jointDamping;

-	float m_jointFriction;

-	float m_jointLowerLimit;

-	float m_jointUpperLimit;

-	float m_jointMaxForce;

-	float m_jointMaxVelocity;

-

-	char *m_linkName;

-	char *m_jointName;

-	btCollisionObjectFloatData *m_linkCollider;

-	char *m_paddingPtr;

-};

-

-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64

-struct btMultiBodyDoubleData

-{

-	btVector3DoubleData m_baseWorldPosition;

-	btQuaternionDoubleData m_baseWorldOrientation;

-	btVector3DoubleData m_baseLinearVelocity;

-	btVector3DoubleData m_baseAngularVelocity;

-	btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)

-	double m_baseMass;

-	int m_numLinks;

-	char m_padding[4];

-

-	char *m_baseName;

-	btMultiBodyLinkDoubleData *m_links;

-	btCollisionObjectDoubleData *m_baseCollider;

-};

-

-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64

-struct btMultiBodyFloatData

-{

-	btVector3FloatData m_baseWorldPosition;

-	btQuaternionFloatData m_baseWorldOrientation;

-	btVector3FloatData m_baseLinearVelocity;

-	btVector3FloatData m_baseAngularVelocity;

-

-	btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)

-	float m_baseMass;

-	int m_numLinks;

-

-	char *m_baseName;

-	btMultiBodyLinkFloatData *m_links;

-	btCollisionObjectFloatData *m_baseCollider;

-};

-

-#endif

+/*
+ * PURPOSE:
+ *   Class representing an articulated rigid body. Stores the body's
+ *   current state, allows forces and torques to be set, handles
+ *   timestepping and implements Featherstone's algorithm.
+ *   
+ * COPYRIGHT:
+ *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
+ *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+ *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as 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_MULTIBODY_H
+#define BT_MULTIBODY_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
+#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;
+
+ATTRIBUTE_ALIGNED16(class)
+btMultiBody
+{
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	//
+	// 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 fixedBase,            // whether the base is fixed (true) or can move (false)
+				bool canSleep, bool deprecatedMultiDof = true);
+
+	virtual ~btMultiBody();
+
+	//note: fixed link collision with parent is always disabled
+	void setupFixed(int linkIndex,
+					btScalar mass,
+					const btVector3 &inertia,
+					int parent,
+					const btQuaternion &rotParentToThis,
+					const btVector3 &parentComToThisPivotOffset,
+					const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);
+
+	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 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 m_links[index];
+	}
+
+	btMultibodyLink &getLink(int index)
+	{
+		return m_links[index];
+	}
+
+	void setBaseCollider(btMultiBodyLinkCollider * collider)  //collider can be NULL to disable collision for the base
+	{
+		m_baseCollider = collider;
+	}
+	const btMultiBodyLinkCollider *getBaseCollider() const
+	{
+		return m_baseCollider;
+	}
+	btMultiBodyLinkCollider *getBaseCollider()
+	{
+		return m_baseCollider;
+	}
+
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	btMultiBodyLinkCollider *getLinkCollider(int index)
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	//
+	// get parent
+	// input: link num from 0 to num_links-1
+	// output: link num from 0 to num_links-1, OR -1 to mean the base.
+	//
+	int getParent(int link_num) const;
+
+	//
+	// get number of m_links, masses, moments of 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) { 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 m_basePos; 
+	}  // in world frame
+	const btVector3 getBaseVel() const
+	{
+		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
+	}  // in world frame
+	const btQuaternion &getWorldToBaseRot() const
+	{
+		return m_baseQuat;
+	}                                                                                               // rotates world vectors into base frame
+	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
+
+	void setBasePos(const btVector3 &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_realBuf[3] = vel[0];
+		m_realBuf[4] = vel[1];
+		m_realBuf[5] = vel[2];
+	}
+	void setWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+	}
+	void setBaseOmega(const btVector3 &omega)
+	{
+		m_realBuf[0] = omega[0];
+		m_realBuf[1] = omega[1];
+		m_realBuf[2] = omega[2];
+	}
+
+	//
+	// 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, const double *q);
+	void setJointVelMultiDof(int i, const double *qdot);
+	void setJointPosMultiDof(int i, const float *q);
+	void setJointVelMultiDof(int i, const float *qdot);
+
+	//
+	// direct access to velocities as a vector of 6 + num_links elements.
+	// (omega first, then v, then joint velocities.)
+	//
+	const btScalar *getVelocityVector() const
+	{
+		return &m_realBuf[0];
+	}
+	/*    btScalar * getVelocityVector() 
+	{ 
+		return &real_buf[0]; 
+	}
+  */
+
+	//
+	// get the frames of reference (positions and orientations) of the child m_links
+	// (i = 0 to num_links-1)
+	//
+
+	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+
+	//
+	// transform vectors in local frame of link i to world frame (or vice versa)
+	//
+	btVector3 localPosToWorld(int i, const btVector3 &vec) const;
+	btVector3 localDirToWorld(int i, const btVector3 &vec) const;
+	btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
+	btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
+
+	//
+	// transform a frame in local coordinate to a frame in world coordinate
+	//
+	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
+
+	//
+	// calculate kinetic energy and angular momentum
+	// useful for debugging.
+	//
+
+	btScalar getKineticEnergy() const;
+	btVector3 getAngularMomentum() const;
+
+	//
+	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
+	//
+
+	void clearForcesAndTorques();
+	void clearConstraintForces();
+
+	void clearVelocities();
+
+	void addBaseForce(const btVector3 &f)
+	{
+		m_baseForce += f;
+	}
+	void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
+	void addLinkForce(int i, const btVector3 &f);
+	void addLinkTorque(int i, const btVector3 &t);
+
+	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);
+
+	//
+	// dynamics routines.
+	//
+
+	// timestep the velocities (given the external forces/torques set using addBaseForce etc).
+	// also sets up caches for calcAccelerationDeltas.
+	//
+	// Note: the caller must provide three vectors which are used as
+	// temporary scratch space. The idea here is to reduce dynamic
+	// memory allocation: the same scratch vectors can be re-used
+	// again and again for different Multibodies, instead of each
+	// btMultiBody allocating (and then deallocating) their own
+	// individual scratch buffers. This gives a considerable speed
+	// improvement, at least on Windows (where dynamic memory
+	// allocation appears to be fairly slow).
+	//
+
+	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
+															  btAlignedObjectArray<btScalar> & scratch_r,
+															  btAlignedObjectArray<btVector3> & scratch_v,
+															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
+
+	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
+	//void stepVelocitiesMultiDof(btScalar dt,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							bool isConstraintPass = false)
+	//{
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+	//}
+
+	// calcAccelerationDeltasMultiDof
+	// input: force vector (in same format as jacobian, i.e.:
+	//                      3 torque values, 3 force values, num_links joint torque values)
+	// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+	// (existing contents of output array are replaced)
+	// calcAccelerationDeltasMultiDof must have been called first.
+	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v) const;
+
+	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 stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+
+	//
+	// contacts
+	//
+
+	// This routine fills out a contact constraint jacobian for this body.
+	// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+	// 'normal' & 'contact_point' are both given in world coordinates.
+
+	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 { fillConstraintJacobianMultiDof(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 fillConstraintJacobianMultiDof(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)
+	{
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
+	}
+
+	bool getCanSleep() const
+	{
+		return m_canSleep;
+	}
+
+	bool getCanWakeup() const
+	{
+		return m_canWakeup;
+	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
+	bool isAwake() const { return m_awake; }
+	void wakeUp();
+	void goToSleep();
+	void checkMotionAndSleepIfRequired(btScalar timestep);
+
+	bool hasFixedBase() const
+	{
+		return m_fixedBase;
+	}
+
+	void setFixedBase(bool fixedBase)
+	{
+		m_fixedBase = fixedBase;
+	}
+
+	int getCompanionId() const
+	{
+		return m_companionId;
+	}
+	void setCompanionId(int id)
+	{
+		//printf("for %p setCompanionId(%d)\n",this, id);
+		m_companionId = id;
+	}
+
+	void setNumLinks(int numLinks)  //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
+	{
+		m_links.resize(numLinks);
+	}
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+	void setLinearDamping(btScalar damp)
+	{
+		m_linearDamping = damp;
+	}
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+	void setAngularDamping(btScalar damp)
+	{
+		m_angularDamping = damp;
+	}
+
+	bool getUseGyroTerm() const
+	{
+		return m_useGyroTerm;
+	}
+	void setUseGyroTerm(bool useGyro)
+	{
+		m_useGyroTerm = useGyro;
+	}
+	btScalar getMaxCoordinateVelocity() const
+	{
+		return m_maxCoordinateVelocity;
+	}
+	void setMaxCoordinateVelocity(btScalar maxVel)
+	{
+		m_maxCoordinateVelocity = maxVel;
+	}
+
+	btScalar getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+	void setHasSelfCollision(bool hasSelfCollision)
+	{
+		m_hasSelfCollision = hasSelfCollision;
+	}
+	bool hasSelfCollision() const
+	{
+		return m_hasSelfCollision;
+	}
+
+	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 compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
+
+	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;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void *getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
+	int getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserPointer(void *userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
+
+
+
+private:
+	btMultiBody(const btMultiBody &);     // not implemented
+	void operator=(const btMultiBody &);  // not implemented
+
+	void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar 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 m_basePos;      // position of COM of base (world frame)
+	btQuaternion m_baseQuat;  // rotates world points into base frame
+
+	btScalar m_baseMass;      // mass of the base
+	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
+
+	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.
+
+	//
+	// realBuf:
+	//  offset         size            array
+	//   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
+	//
+	// vectorBuf:
+	//  offset         size         array
+	//   0              num_links    h_top
+	//   num_links      num_links    h_bottom
+	//
+	// matrixBuf:
+	//  offset         size         array
+	//   0              num_links+1  rot_from_parent
+	//
+	btAlignedObjectArray<btScalar> m_deltaV;
+	btAlignedObjectArray<btScalar> m_realBuf;
+	btAlignedObjectArray<btVector3> m_vectorBuf;
+	btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
+
+	btMatrix3x3 m_cachedInertiaTopLeft;
+	btMatrix3x3 m_cachedInertiaTopRight;
+	btMatrix3x3 m_cachedInertiaLowerLeft;
+	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
+
+	bool m_fixedBase;
+
+	// Sleep parameters.
+	bool m_awake;
+	bool m_canSleep;
+	bool m_canWakeup;
+	btScalar m_sleepTimer;
+
+	void *m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
+	int m_companionId;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+	bool m_useGyroTerm;
+	btScalar m_maxAppliedImpulse;
+	btScalar m_maxCoordinateVelocity;
+	bool m_hasSelfCollision;
+
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
+
+	bool m_useRK4, m_useGlobalVelocities;
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
+	///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_parentComToThisPivotOffset;
+	btVector3DoubleData m_thisPivotToThisComOffset;
+	btVector3DoubleData m_jointAxisTop[6];
+	btVector3DoubleData m_jointAxisBottom[6];
+
+	btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData m_absFrameTotVelocityTop;
+	btVector3DoubleData m_absFrameTotVelocityBottom;
+	btVector3DoubleData m_absFrameLocVelocityTop;
+	btVector3DoubleData m_absFrameLocVelocityBottom;
+
+	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];
+
+	double m_jointDamping;
+	double m_jointFriction;
+	double m_jointLowerLimit;
+	double m_jointUpperLimit;
+	double m_jointMaxForce;
+	double m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectDoubleData *m_linkCollider;
+	char *m_paddingPtr;
+};
+
+struct btMultiBodyLinkFloatData
+{
+	btQuaternionFloatData m_zeroRotParentToThis;
+	btVector3FloatData m_parentComToThisPivotOffset;
+	btVector3FloatData m_thisPivotToThisComOffset;
+	btVector3FloatData m_jointAxisTop[6];
+	btVector3FloatData m_jointAxisBottom[6];
+	btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3FloatData m_absFrameTotVelocityTop;
+	btVector3FloatData m_absFrameTotVelocityBottom;
+	btVector3FloatData m_absFrameLocVelocityTop;
+	btVector3FloatData m_absFrameLocVelocityBottom;
+
+	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;
+	float m_jointDamping;
+	float m_jointFriction;
+	float m_jointLowerLimit;
+	float m_jointUpperLimit;
+	float m_jointMaxForce;
+	float m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectFloatData *m_linkCollider;
+	char *m_paddingPtr;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btMultiBodyDoubleData
+{
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
+	btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	double m_baseMass;
+	int m_numLinks;
+	char m_padding[4];
+
+	char *m_baseName;
+	btMultiBodyLinkDoubleData *m_links;
+	btCollisionObjectDoubleData *m_baseCollider;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btMultiBodyFloatData
+{
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	float m_baseMass;
+	int m_numLinks;
+
+	char *m_baseName;
+	btMultiBodyLinkFloatData *m_links;
+	btCollisionObjectFloatData *m_baseCollider;
+};
+
+#endif
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
index fc0b86958..5ef9444c2 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -1,215 +1,215 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

-3. This notice may not be removed or altered from any source distribution.

-*/

-

-///This file was written by Erwin Coumans

-

-#include "btMultiBodyFixedConstraint.h"

-#include "btMultiBodyLinkCollider.h"

-#include "BulletDynamics/Dynamics/btRigidBody.h"

-#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"

-#include "LinearMath/btIDebugDraw.h"

-

-#define BTMBFIXEDCONSTRAINT_DIM 6

-

-btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)

-	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),

-	  m_rigidBodyA(0),

-	  m_rigidBodyB(bodyB),

-	  m_pivotInA(pivotInA),

-	  m_pivotInB(pivotInB),

-	  m_frameInA(frameInA),

-	  m_frameInB(frameInB)

-{

-	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses

-}

-

-btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)

-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),

-	  m_rigidBodyA(0),

-	  m_rigidBodyB(0),

-	  m_pivotInA(pivotInA),

-	  m_pivotInB(pivotInB),

-	  m_frameInA(frameInA),

-	  m_frameInB(frameInB)

-{

-	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses

-}

-

-void btMultiBodyFixedConstraint::finalizeMultiDof()

-{

-	//not implemented yet

-	btAssert(0);

-}

-

-btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()

-{

-}

-

-int btMultiBodyFixedConstraint::getIslandIdA() const

-{

-	if (m_rigidBodyA)

-		return m_rigidBodyA->getIslandTag();

-

-	if (m_bodyA)

-	{

-		if (m_linkA < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyA->getLink(m_linkA).m_collider)

-				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-int btMultiBodyFixedConstraint::getIslandIdB() const

-{

-	if (m_rigidBodyB)

-		return m_rigidBodyB->getIslandTag();

-	if (m_bodyB)

-	{

-		if (m_linkB < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyB->getLink(m_linkB).m_collider)

-				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)

-{

-	int numDim = BTMBFIXEDCONSTRAINT_DIM;

-	for (int i = 0; i < numDim; i++)

-	{

-		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();

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

-

-		// Convert local points back to world

-		btVector3 pivotAworld = m_pivotInA;

-		btMatrix3x3 frameAworld = m_frameInA;

-		if (m_rigidBodyA)

-		{

-			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();

-			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;

-			frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());

-		}

-		else

-		{

-			if (m_bodyA)

-			{

-				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);

-				frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);

-			}

-		}

-		btVector3 pivotBworld = m_pivotInB;

-		btMatrix3x3 frameBworld = m_frameInB;

-		if (m_rigidBodyB)

-		{

-			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();

-			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;

-			frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());

-		}

-		else

-		{

-			if (m_bodyB)

-			{

-				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);

-				frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);

-			}

-		}

-

-		btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;

-		btVector3 angleDiff;

-		btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);

-

-		btVector3 constraintNormalLin(0, 0, 0);

-		btVector3 constraintNormalAng(0, 0, 0);

-		btScalar posError = 0.0;

-		if (i < 3)

-		{

-			constraintNormalLin[i] = 1;

-			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);

-			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,

-									constraintNormalLin, pivotAworld, pivotBworld,

-									posError,

-									infoGlobal,

-									-m_maxAppliedImpulse, m_maxAppliedImpulse);

-		}

-		else

-		{  //i>=3

-			constraintNormalAng = frameAworld.getColumn(i % 3);

-			posError = angleDiff[i % 3];

-			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,

-									constraintNormalLin, pivotAworld, pivotBworld,

-									posError,

-									infoGlobal,

-									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);

-		}

-	}

-}

-

-void btMultiBodyFixedConstraint::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);

-	}

-}

+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	int numDim = BTMBFIXEDCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		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;
+
+		// Convert local points back to world
+		btVector3 pivotAworld = m_pivotInA;
+		btMatrix3x3 frameAworld = m_frameInA;
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+			frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		}
+		else
+		{
+			if (m_bodyA)
+			{
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+				frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+			}
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		btMatrix3x3 frameBworld = m_frameInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+			frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+		}
+		else
+		{
+			if (m_bodyB)
+			{
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+				frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+			}
+		}
+
+		btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+		btVector3 angleDiff;
+		btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 3)
+		{
+			constraintNormalLin[i] = 1;
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=3
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodyFixedConstraint::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);
+	}
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index cc7156c64..5c816c498 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -1,183 +1,183 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

-3. This notice may not be removed or altered from any source distribution.

-*/

-

-///This file was written by Erwin Coumans

-

-#include "btMultiBodyJointMotor.h"

-#include "btMultiBody.h"

-#include "btMultiBodyLinkCollider.h"

-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

-

-btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)

-	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),

-	  m_desiredVelocity(desiredVelocity),

-	  m_desiredPosition(0),

-	  m_kd(1.),

-	  m_kp(0),

-	  m_erp(1),

-	  m_rhsClamp(SIMD_INFINITY)

-{

-	m_maxAppliedImpulse = maxMotorImpulse;

-	// the data.m_jacobians never change, so may as well

-	// initialize them here

-}

-

-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->getLink(m_linkA).m_dofOffset + linkDoF);

-

-	// 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),

-	  m_desiredPosition(0),

-	  m_kd(1.),

-	  m_kp(0),

-	  m_erp(1),

-	  m_rhsClamp(SIMD_INFINITY)

-{

-	btAssert(linkDoF < body->getLink(link).m_dofCount);

-

-	m_maxAppliedImpulse = maxMotorImpulse;

-}

-btMultiBodyJointMotor::~btMultiBodyJointMotor()

-{

-}

-

-int btMultiBodyJointMotor::getIslandIdA() const

-{

-	if (this->m_linkA < 0)

-	{

-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();

-		if (col)

-			return col->getIslandTag();

-	}

-	else

-	{

-		if (m_bodyA->getLink(m_linkA).m_collider)

-		{

-			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-int btMultiBodyJointMotor::getIslandIdB() const

-{

-	if (m_linkB < 0)

-	{

-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();

-		if (col)

-			return col->getIslandTag();

-	}

-	else

-	{

-		if (m_bodyB->getLink(m_linkB).m_collider)

-		{

-			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,

-												 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();

-	}

-

-	//don't crash

-	if (m_numDofsFinalized != m_jacSizeBoth)

-		return;

-

-	if (m_maxAppliedImpulse == 0.f)

-		return;

-

-	const btScalar posError = 0;

-	const btVector3 dummy(0, 0, 0);

-

-	for (int row = 0; row < getNumRows(); row++)

-	{

-		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();

-

-		int dof = 0;

-		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];

-		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];

-		btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;

-

-		btScalar velocityError = (m_desiredVelocity - currentVelocity);

-		btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;

-		if (rhs > m_rhsClamp)

-		{

-			rhs = m_rhsClamp;

-		}

-		if (rhs < -m_rhsClamp)

-		{

-			rhs = -m_rhsClamp;

-		}

-

-		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);

-		constraintRow.m_orgConstraint = this;

-		constraintRow.m_orgDofIndex = row;

-		{

-			//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);

-				}

-			};

-		}

-	}

-}

+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
+{
+	m_maxAppliedImpulse = maxMotorImpulse;
+	// the data.m_jacobians never change, so may as well
+	// initialize them here
+}
+
+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->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// 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),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
+{
+	btAssert(linkDoF < body->getLink(link).m_dofCount);
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+btMultiBodyJointMotor::~btMultiBodyJointMotor()
+{
+}
+
+int btMultiBodyJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 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();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
+
+		btScalar velocityError = (m_desiredVelocity - currentVelocity);
+		btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
+		if (rhs > m_rhsClamp)
+		{
+			rhs = m_rhsClamp;
+		}
+		if (rhs < -m_rhsClamp)
+		{
+			rhs = -m_rhsClamp;
+		}
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//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);
+				}
+			};
+		}
+	}
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
index 8d7210600..1aca36352 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -1,77 +1,77 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

-3. This notice may not be removed or altered from any source distribution.

-*/

-

-///This file was written by Erwin Coumans

-

-#ifndef BT_MULTIBODY_JOINT_MOTOR_H

-#define BT_MULTIBODY_JOINT_MOTOR_H

-

-#include "btMultiBodyConstraint.h"

-struct btSolverInfo;

-

-class btMultiBodyJointMotor : public btMultiBodyConstraint

-{

-protected:

-	btScalar m_desiredVelocity;

-	btScalar m_desiredPosition;

-	btScalar m_kd;

-	btScalar m_kp;

-	btScalar m_erp;

-	btScalar m_rhsClamp;  //maximum error

-

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

-

-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,

-									  btMultiBodyJacobianData& data,

-									  const btContactSolverInfo& infoGlobal);

-

-	virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)

-	{

-		m_desiredVelocity = velTarget;

-		m_kd = kd;

-	}

-

-	virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)

-	{

-		m_desiredPosition = posTarget;

-		m_kp = kp;

-	}

-

-	virtual void setErp(btScalar erp)

-	{

-		m_erp = erp;

-	}

-	virtual btScalar getErp() const

-	{

-		return m_erp;

-	}

-	virtual void setRhsClamp(btScalar rhsClamp)

-	{

-		m_rhsClamp = rhsClamp;

-	}

-	virtual void debugDraw(class btIDebugDraw* drawer)

-	{

-		//todo(erwincoumans)

-	}

-};

-

-#endif  //BT_MULTIBODY_JOINT_MOTOR_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_JOINT_MOTOR_H
+#define BT_MULTIBODY_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodyJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btScalar m_desiredVelocity;
+	btScalar m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+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;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_JOINT_MOTOR_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index ee0fc193b..92d41dfac 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -1,239 +1,239 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as 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_MULTIBODY_LINK_H

-#define BT_MULTIBODY_LINK_H

-

-#include "LinearMath/btQuaternion.h"

-#include "LinearMath/btVector3.h"

-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

-

-enum btMultiBodyLinkFlags

-{

-	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,

-	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,

-};

-

-//both defines are now permanently enabled

-#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS

-#define TEST_SPATIAL_ALGEBRA_LAYER

-

-//

-// Various spatial helper functions

-//

-

-//namespace {

-

-#include "LinearMath/btSpatialAlgebra.h"

-

-//}

-

-//

-// Link struct

-//

-

-struct btMultibodyLink

-{

-	BT_DECLARE_ALIGNED_ALLOCATOR();

-

-	btScalar m_mass;           // mass of link

-	btVector3 m_inertiaLocal;  // inertia of link (local frame; diagonal)

-

-	int m_parent;  // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.

-

-	btQuaternion m_zeroRotParentToThis;  // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.

-

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

-

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

-	const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!

-

-	btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.

-	btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.

-	btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.

-	btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.

-	btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.

-	btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.

-

-	// ctor: set some sensible defaults

-	btMultibodyLink()

-		: 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_dofCount(0),

-		  m_posVarCount(0),

-		  m_jointType(btMultibodyLink::eInvalid),

-		  m_jointFeedback(0),

-		  m_linkName(0),

-		  m_jointName(0),

-		  m_userPtr(0),

-		  m_jointDamping(0),

-		  m_jointFriction(0),

-		  m_jointLowerLimit(0),

-		  m_jointUpperLimit(0),

-		  m_jointMaxForce(0),

-		  m_jointMaxVelocity(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_appliedConstraintForce.setValue(0, 0, 0);

-		m_appliedConstraintTorque.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 m_cachedRotParentToThis and m_cachedRVector

-	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);

-			}

-		}

-	}

-};

-

-#endif  //BT_MULTIBODY_LINK_H

+/*
+Bullet Continuous Collision Detection and Physics Library
+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, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_MULTIBODY_LINK_H
+#define BT_MULTIBODY_LINK_H
+
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btVector3.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+enum btMultiBodyLinkFlags
+{
+	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
+	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
+};
+
+//both defines are now permanently enabled
+#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define TEST_SPATIAL_ALGEBRA_LAYER
+
+//
+// Various spatial helper functions
+//
+
+//namespace {
+
+#include "LinearMath/btSpatialAlgebra.h"
+
+//}
+
+//
+// Link struct
+//
+
+struct btMultibodyLink
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btScalar m_mass;           // mass of link
+	btVector3 m_inertiaLocal;  // inertia of link (local frame; diagonal)
+
+	int m_parent;  // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+
+	btQuaternion m_zeroRotParentToThis;  // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+
+	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;
+
+	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!
+	const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!
+
+	btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+	btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
+	btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+
+	// ctor: set some sensible defaults
+	btMultibodyLink()
+		: 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_dofCount(0),
+		  m_posVarCount(0),
+		  m_jointType(btMultibodyLink::eInvalid),
+		  m_jointFeedback(0),
+		  m_linkName(0),
+		  m_jointName(0),
+		  m_userPtr(0),
+		  m_jointDamping(0),
+		  m_jointFriction(0),
+		  m_jointLowerLimit(0),
+		  m_jointUpperLimit(0),
+		  m_jointMaxForce(0),
+		  m_jointMaxVelocity(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_appliedConstraintForce.setValue(0, 0, 0);
+		m_appliedConstraintTorque.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 m_cachedRotParentToThis and m_cachedRVector
+	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);
+			}
+		}
+	}
+};
+
+#endif  //BT_MULTIBODY_LINK_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index b61ae1d77..37d3aede3 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -1,216 +1,216 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

-3. This notice may not be removed or altered from any source distribution.

-*/

-

-///This file was written by Erwin Coumans

-

-#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, 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, 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()

-{

-}

-

-int btMultiBodyPoint2Point::getIslandIdA() const

-{

-	if (m_rigidBodyA)

-		return m_rigidBodyA->getIslandTag();

-

-	if (m_bodyA)

-	{

-		if (m_linkA < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyA->getLink(m_linkA).m_collider)

-				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-int btMultiBodyPoint2Point::getIslandIdB() const

-{

-	if (m_rigidBodyB)

-		return m_rigidBodyB->getIslandTag();

-	if (m_bodyB)

-	{

-		if (m_linkB < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyB->getLink(m_linkB).m_collider)

-				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,

-												  btMultiBodyJacobianData& data,

-												  const btContactSolverInfo& infoGlobal)

-{

-	//	int i=1;

-	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

-

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

-		{

-			if (m_bodyA)

-				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);

-		}

-		btVector3 pivotBworld = m_pivotInB;

-		if (m_rigidBodyB)

-		{

-			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();

-			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;

-		}

-		else

-		{

-			if (m_bodyB)

-				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);

-		}

-

-		btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;

-

-#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST

-

-		fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 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);

-	}

-}

+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#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, 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, 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()
+{
+}
+
+int btMultiBodyPoint2Point::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyPoint2Point::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												  btMultiBodyJacobianData& data,
+												  const btContactSolverInfo& infoGlobal)
+{
+	//	int i=1;
+	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
+
+		// 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
+		{
+			if (m_bodyA)
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		}
+		else
+		{
+			if (m_bodyB)
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		}
+
+		btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
+
+#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
+		fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 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);
+	}
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
index addd5bf89..e025302ce 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
@@ -1,234 +1,234 @@
-/*

-Bullet Continuous Collision Detection and Physics Library

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

-subject to the following restrictions:

-

-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

-3. This notice may not be removed or altered from any source distribution.

-*/

-

-///This file was written by Erwin Coumans

-

-#include "btMultiBodySliderConstraint.h"

-#include "btMultiBodyLinkCollider.h"

-#include "BulletDynamics/Dynamics/btRigidBody.h"

-#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"

-#include "LinearMath/btIDebugDraw.h"

-

-#define BTMBSLIDERCONSTRAINT_DIM 5

-#define EPSILON 0.000001

-

-btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)

-	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),

-	  m_rigidBodyA(0),

-	  m_rigidBodyB(bodyB),

-	  m_pivotInA(pivotInA),

-	  m_pivotInB(pivotInB),

-	  m_frameInA(frameInA),

-	  m_frameInB(frameInB),

-	  m_jointAxis(jointAxis)

-{

-	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses

-}

-

-btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)

-	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),

-	  m_rigidBodyA(0),

-	  m_rigidBodyB(0),

-	  m_pivotInA(pivotInA),

-	  m_pivotInB(pivotInB),

-	  m_frameInA(frameInA),

-	  m_frameInB(frameInB),

-	  m_jointAxis(jointAxis)

-{

-	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses

-}

-

-void btMultiBodySliderConstraint::finalizeMultiDof()

-{

-	//not implemented yet

-	btAssert(0);

-}

-

-btMultiBodySliderConstraint::~btMultiBodySliderConstraint()

-{

-}

-

-int btMultiBodySliderConstraint::getIslandIdA() const

-{

-	if (m_rigidBodyA)

-		return m_rigidBodyA->getIslandTag();

-

-	if (m_bodyA)

-	{

-		if (m_linkA < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyA->getLink(m_linkA).m_collider)

-				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-

-int btMultiBodySliderConstraint::getIslandIdB() const

-{

-	if (m_rigidBodyB)

-		return m_rigidBodyB->getIslandTag();

-	if (m_bodyB)

-	{

-		if (m_linkB < 0)

-		{

-			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();

-			if (col)

-				return col->getIslandTag();

-		}

-		else

-		{

-			if (m_bodyB->getLink(m_linkB).m_collider)

-				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();

-		}

-	}

-	return -1;

-}

-void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)

-{

-	// Convert local points back to world

-	btVector3 pivotAworld = m_pivotInA;

-	btMatrix3x3 frameAworld = m_frameInA;

-	btVector3 jointAxis = m_jointAxis;

-	if (m_rigidBodyA)

-	{

-		pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;

-		frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());

-		jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);

-	}

-	else if (m_bodyA)

-	{

-		pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);

-		frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);

-		jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);

-	}

-	btVector3 pivotBworld = m_pivotInB;

-	btMatrix3x3 frameBworld = m_frameInB;

-	if (m_rigidBodyB)

-	{

-		pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;

-		frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());

-	}

-	else if (m_bodyB)

-	{

-		pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);

-		frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);

-	}

-

-	btVector3 constraintAxis[2];

-	for (int i = 0; i < 3; ++i)

-	{

-		constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);

-		if (constraintAxis[0].safeNorm() > EPSILON)

-		{

-			constraintAxis[0] = constraintAxis[0].normalized();

-			constraintAxis[1] = jointAxis.cross(constraintAxis[0]);

-			constraintAxis[1] = constraintAxis[1].normalized();

-			break;

-		}

-	}

-

-	btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;

-	btVector3 angleDiff;

-	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);

-

-	int numDim = BTMBSLIDERCONSTRAINT_DIM;

-	for (int i = 0; i < numDim; i++)

-	{

-		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();

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

-

-		if (m_rigidBodyA)

-		{

-			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();

-		}

-		if (m_rigidBodyB)

-		{

-			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();

-		}

-

-		btVector3 constraintNormalLin(0, 0, 0);

-		btVector3 constraintNormalAng(0, 0, 0);

-		btScalar posError = 0.0;

-		if (i < 2)

-		{

-			constraintNormalLin = constraintAxis[i];

-			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);

-			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,

-									constraintNormalLin, pivotAworld, pivotBworld,

-									posError,

-									infoGlobal,

-									-m_maxAppliedImpulse, m_maxAppliedImpulse);

-		}

-		else

-		{  //i>=2

-			constraintNormalAng = frameAworld.getColumn(i % 3);

-			posError = angleDiff[i % 3];

-			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,

-									constraintNormalLin, pivotAworld, pivotBworld,

-									posError,

-									infoGlobal,

-									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);

-		}

-	}

-}

-

-void btMultiBodySliderConstraint::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);

-	}

-}

+/*
+Bullet Continuous Collision Detection and Physics Library
+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,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySliderConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBSLIDERCONSTRAINT_DIM 5
+#define EPSILON 0.000001
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodySliderConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
+{
+}
+
+int btMultiBodySliderConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySliderConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	// Convert local points back to world
+	btVector3 pivotAworld = m_pivotInA;
+	btMatrix3x3 frameAworld = m_frameInA;
+	btVector3 jointAxis = m_jointAxis;
+	if (m_rigidBodyA)
+	{
+		pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
+	}
+	else if (m_bodyA)
+	{
+		pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
+		jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
+	}
+	btVector3 pivotBworld = m_pivotInB;
+	btMatrix3x3 frameBworld = m_frameInB;
+	if (m_rigidBodyB)
+	{
+		pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+	}
+	else if (m_bodyB)
+	{
+		pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
+	}
+
+	btVector3 constraintAxis[2];
+	for (int i = 0; i < 3; ++i)
+	{
+		constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
+		if (constraintAxis[0].safeNorm() > EPSILON)
+		{
+			constraintAxis[0] = constraintAxis[0].normalized();
+			constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
+			constraintAxis[1] = constraintAxis[1].normalized();
+			break;
+		}
+	}
+
+	btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+	int numDim = BTMBSLIDERCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		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;
+
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+		}
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+		}
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 2)
+		{
+			constraintNormalLin = constraintAxis[i];
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=2
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodySliderConstraint::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);
+	}
+}
diff --git a/src/BulletSoftBody/premake4.lua b/src/BulletSoftBody/premake4.lua
index a75e33619..6f09196fa 100644
--- a/src/BulletSoftBody/premake4.lua
+++ b/src/BulletSoftBody/premake4.lua
@@ -1,14 +1,14 @@
-	project "BulletSoftBody"

-		

-	kind "StaticLib"

-	

-	includedirs {

-		"..",

-	}

-    if os.is("Linux") then

-        buildoptions{"-fPIC"}

-    end

-	files {

-		"**.cpp",

-		"**.h"

+	project "BulletSoftBody"
+		
+	kind "StaticLib"
+	
+	includedirs {
+		"..",
+	}
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+	files {
+		"**.cpp",
+		"**.h"
 	}
\ No newline at end of file