Merge pull request #12641 from AndreaCatania/bullet

Bullet physics wrapper

modules/bullet/SCsub

@@ -0,0 +1,191 @@
+#!/usr/bin/env python
+
+Import('env')
+
+# build only version 2
+# Bullet 2.87
+bullet_src__2_x = [
+        # BulletCollision
+          "BulletCollision/BroadphaseCollision/btAxisSweep3.cpp"
+        , "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp"
+        , "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp"
+        , "BulletCollision/BroadphaseCollision/btDbvt.cpp"
+        , "BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp"
+        , "BulletCollision/BroadphaseCollision/btDispatcher.cpp"
+        , "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp"
+        , "BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp"
+        , "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp"
+        , "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp"
+        , "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp"
+        , "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp"
+        , "BulletCollision/CollisionDispatch/btCollisionObject.cpp"
+        , "BulletCollision/CollisionDispatch/btCollisionWorld.cpp"
+        , "BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp"
+        , "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp"
+        , "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btGhostObject.cpp"
+        , "BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp"
+        , "BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp"
+        , "BulletCollision/CollisionDispatch/btManifoldResult.cpp"
+        , "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp"
+        , "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp"
+        , "BulletCollision/CollisionDispatch/btUnionFind.cpp"
+        , "BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp"
+        , "BulletCollision/CollisionShapes/btBoxShape.cpp"
+        , "BulletCollision/CollisionShapes/btBox2dShape.cpp"
+        , "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btCapsuleShape.cpp"
+        , "BulletCollision/CollisionShapes/btCollisionShape.cpp"
+        , "BulletCollision/CollisionShapes/btCompoundShape.cpp"
+        , "BulletCollision/CollisionShapes/btConcaveShape.cpp"
+        , "BulletCollision/CollisionShapes/btConeShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvexHullShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvexInternalShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvexPolyhedron.cpp"
+        , "BulletCollision/CollisionShapes/btConvexShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvex2dShape.cpp"
+        , "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btCylinderShape.cpp"
+        , "BulletCollision/CollisionShapes/btEmptyShape.cpp"
+        , "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
+        , "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
+        , "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
+        , "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
+        , "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp"
+        , "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btShapeHull.cpp"
+        , "BulletCollision/CollisionShapes/btSphereShape.cpp"
+        , "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp"
+        , "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp"
+        , "BulletCollision/CollisionShapes/btTetrahedronShape.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleBuffer.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleCallback.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleMesh.cpp"
+        , "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp"
+        , "BulletCollision/CollisionShapes/btUniformScalingShape.cpp"
+        , "BulletCollision/Gimpact/btContactProcessing.cpp"
+        , "BulletCollision/Gimpact/btGenericPoolAllocator.cpp"
+        , "BulletCollision/Gimpact/btGImpactBvh.cpp"
+        , "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp"
+        , "BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp"
+        , "BulletCollision/Gimpact/btGImpactShape.cpp"
+        , "BulletCollision/Gimpact/btTriangleShapeEx.cpp"
+        , "BulletCollision/Gimpact/gim_box_set.cpp"
+        , "BulletCollision/Gimpact/gim_contact.cpp"
+        , "BulletCollision/Gimpact/gim_memory.cpp"
+        , "BulletCollision/Gimpact/gim_tri_collision.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp"
+        , "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp"
+
+        # BulletDynamics
+        , "BulletDynamics/Character/btKinematicCharacterController.cpp"
+        , "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btContactConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btFixedConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btGearConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp"
+        , "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp"
+        , "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp"
+        , "BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp"
+        , "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp"
+        , "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp"
+        , "BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp"
+        , "BulletDynamics/Dynamics/btRigidBody.cpp"
+        , "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp"
+        #, "BulletDynamics/Dynamics/Bullet-C-API.cpp"
+        , "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
+        , "BulletDynamics/Vehicle/btWheelInfo.cpp"
+        , "BulletDynamics/Featherstone/btMultiBody.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"
+        , "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
+        , "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp"
+        , "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp"
+        , "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp"
+
+        # BulletInverseDynamics
+        , "BulletInverseDynamics/IDMath.cpp"
+        , "BulletInverseDynamics/MultiBodyTree.cpp"
+        , "BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp"
+        , "BulletInverseDynamics/details/MultiBodyTreeImpl.cpp"
+
+        # BulletSoftBody
+        , "BulletSoftBody/btSoftBody.cpp"
+        , "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp"
+        , "BulletSoftBody/btSoftBodyHelpers.cpp"
+        , "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp"
+        , "BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp"
+        , "BulletSoftBody/btSoftRigidDynamicsWorld.cpp"
+        , "BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp"
+        , "BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp"
+        , "BulletSoftBody/btDefaultSoftBodySolver.cpp"
+
+        # clew
+        , "clew/clew.c"
+
+        # LinearMath
+        , "LinearMath/btAlignedAllocator.cpp"
+        , "LinearMath/btConvexHull.cpp"
+        , "LinearMath/btConvexHullComputer.cpp"
+        , "LinearMath/btGeometryUtil.cpp"
+        , "LinearMath/btPolarDecomposition.cpp"
+        , "LinearMath/btQuickprof.cpp"
+        , "LinearMath/btSerializer.cpp"
+        , "LinearMath/btSerializer64.cpp"
+        , "LinearMath/btThreads.cpp"
+        , "LinearMath/btVector3.cpp"
+    ]
+
+thirdparty_dir = "#thirdparty/bullet/"
+thirdparty_src = thirdparty_dir + "src/"
+
+bullet_sources = [thirdparty_src + file for file in bullet_src__2_x]
+
+# include headers
+env.Append(CPPPATH=[thirdparty_src])
+
+env.add_source_files(env.modules_sources, bullet_sources)
+
+# Godot source files
+env.add_source_files(env.modules_sources, "*.cpp")
+
+Export('env')

modules/bullet/SCsub_with_lib

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+
+Import('env')
+
+thirdparty_dir = "#thirdparty/bullet/"
+thirdparty_lib = thirdparty_dir + "Win64/lib/"
+
+bullet_libs = [
+        "Bullet2FileLoader",
+        "Bullet3Collision",
+        "Bullet3Common",
+        "Bullet3Dynamics",
+        "Bullet3Geometry",
+        "Bullet3OpenCL_clew",
+        "BulletCollision",
+        "BulletDynamics",
+        "BulletInverseDynamics",
+        "BulletSoftBody",
+        "LinearMath"
+    ]
+
+thirdparty_src = thirdparty_dir + "src/"
+# include headers
+env.Append(CPPPATH=[thirdparty_src])
+
+# lib
+env.Append(LIBPATH=[thirdparty_dir + "/Win64/lib/"])
+
+bullet_libs = [file+'.lib' for file in bullet_libs]
+# LIBS doesn't work in windows
+env.Append(LINKFLAGS=bullet_libs)
+
+env.add_source_files(env.modules_sources, "*.cpp")

modules/bullet/area_bullet.cpp

@@ -0,0 +1,284 @@
+/*************************************************************************/
+/*  area_bullet.cpp                                                      */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "area_bullet.h"
+#include "BulletCollision/CollisionDispatch/btGhostObject.h"
+#include "btBulletCollisionCommon.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "collision_object_bullet.h"
+#include "space_bullet.h"
+
+AreaBullet::AreaBullet()
+	: RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_AREA),
+	  monitorable(true),
+	  isScratched(false),
+	  spOv_mode(PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED),
+	  spOv_gravityPoint(false),
+	  spOv_gravityPointDistanceScale(0),
+	  spOv_gravityPointAttenuation(1),
+	  spOv_gravityVec(0, -1, 0),
+	  spOv_gravityMag(10),
+	  spOv_linearDump(0.1),
+	  spOv_angularDump(1),
+	  spOv_priority(0) {
+
+	btGhost = bulletnew(btGhostObject);
+	btGhost->setCollisionShape(compoundShape);
+	setupBulletCollisionObject(btGhost);
+	/// Collision objects with a callback still have collision response with dynamic rigid bodies.
+	/// In order to use collision objects as trigger, you have to disable the collision response.
+	set_collision_enabled(false);
+
+	for (int i = 0; i < 5; ++i)
+		call_event_res_ptr[i] = &call_event_res[i];
+}
+
+AreaBullet::~AreaBullet() {
+	remove_all_overlapping_instantly();
+}
+
+void AreaBullet::dispatch_callbacks() {
+	if (!isScratched)
+		return;
+	isScratched = false;
+
+	// Reverse order because I've to remove EXIT objects
+	for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
+		OverlappingObjectData &otherObj = overlappingObjects[i];
+
+		switch (otherObj.state) {
+			case OVERLAP_STATE_ENTER:
+				otherObj.state = OVERLAP_STATE_INSIDE;
+				call_event(otherObj.object, PhysicsServer::AREA_BODY_ADDED);
+				otherObj.object->on_enter_area(this);
+				break;
+			case OVERLAP_STATE_EXIT:
+				call_event(otherObj.object, PhysicsServer::AREA_BODY_REMOVED);
+				otherObj.object->on_exit_area(this);
+				overlappingObjects.remove(i); // Remove after callback
+				break;
+		}
+	}
+}
+
+void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer::AreaBodyStatus p_status) {
+
+	InOutEventCallback &event = eventsCallbacks[static_cast<int>(p_otherObject->getType())];
+	Object *areaGodoObject = ObjectDB::get_instance(event.event_callback_id);
+
+	if (!areaGodoObject) {
+		event.event_callback_id = 0;
+		return;
+	}
+
+	call_event_res[0] = p_status;
+	call_event_res[1] = p_otherObject->get_self(); // Other body
+	call_event_res[2] = p_otherObject->get_instance_id(); // instance ID
+	call_event_res[3] = 0; // other_body_shape ID
+	call_event_res[4] = 0; // self_shape ID
+
+	Variant::CallError outResp;
+	areaGodoObject->call(event.event_callback_method, (const Variant **)call_event_res_ptr, 5, outResp);
+}
+
+void AreaBullet::scratch() {
+	if (isScratched)
+		return;
+	isScratched = true;
+}
+
+void AreaBullet::remove_all_overlapping_instantly() {
+	CollisionObjectBullet *supportObject;
+	for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
+		supportObject = overlappingObjects[i].object;
+		call_event(supportObject, PhysicsServer::AREA_BODY_REMOVED);
+		supportObject->on_exit_area(this);
+	}
+	overlappingObjects.clear();
+}
+
+void AreaBullet::remove_overlapping_instantly(CollisionObjectBullet *p_object) {
+	CollisionObjectBullet *supportObject;
+	for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
+		supportObject = overlappingObjects[i].object;
+		if (supportObject == p_object) {
+			call_event(supportObject, PhysicsServer::AREA_BODY_REMOVED);
+			supportObject->on_exit_area(this);
+			overlappingObjects.remove(i);
+			break;
+		}
+	}
+}
+
+int AreaBullet::find_overlapping_object(CollisionObjectBullet *p_colObj) {
+	const int size = overlappingObjects.size();
+	for (int i = 0; i < size; ++i) {
+		if (overlappingObjects[i].object == p_colObj) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+void AreaBullet::set_monitorable(bool p_monitorable) {
+	monitorable = p_monitorable;
+}
+
+bool AreaBullet::is_monitoring() const {
+	return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
+}
+
+void AreaBullet::reload_body() {
+	if (space) {
+		space->remove_area(this);
+		space->add_area(this);
+	}
+}
+
+void AreaBullet::set_space(SpaceBullet *p_space) {
+	// Clear the old space if there is one
+	if (space) {
+		isScratched = false;
+
+		// Remove this object form the physics world
+		space->remove_area(this);
+	}
+
+	space = p_space;
+
+	if (space) {
+		space->add_area(this);
+	}
+}
+
+void AreaBullet::on_collision_filters_change() {
+	if (space) {
+		space->reload_collision_filters(this);
+	}
+}
+
+void AreaBullet::add_overlap(CollisionObjectBullet *p_otherObject) {
+	scratch();
+	overlappingObjects.push_back(OverlappingObjectData(p_otherObject, OVERLAP_STATE_ENTER));
+	p_otherObject->notify_new_overlap(this);
+}
+
+void AreaBullet::put_overlap_as_exit(int p_index) {
+	scratch();
+	overlappingObjects[p_index].state = OVERLAP_STATE_EXIT;
+}
+
+void AreaBullet::put_overlap_as_inside(int p_index) {
+	// This check is required to be sure this body was inside
+	if (OVERLAP_STATE_DIRTY == overlappingObjects[p_index].state) {
+		overlappingObjects[p_index].state = OVERLAP_STATE_INSIDE;
+	}
+}
+
+void AreaBullet::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY:
+			set_spOv_gravityMag(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
+			set_spOv_gravityVec(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
+			set_spOv_linearDump(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
+			set_spOv_angularDump(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_PRIORITY:
+			set_spOv_priority(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
+			set_spOv_gravityPoint(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+			set_spOv_gravityPointDistanceScale(p_value);
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			set_spOv_gravityPointAttenuation(p_value);
+			break;
+		default:
+			print_line("The Bullet areas dosn't suppot this param: " + itos(p_param));
+	}
+}
+
+Variant AreaBullet::get_param(PhysicsServer::AreaParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY:
+			return spOv_gravityMag;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
+			return spOv_gravityVec;
+		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
+			return spOv_linearDump;
+		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
+			return spOv_angularDump;
+		case PhysicsServer::AREA_PARAM_PRIORITY:
+			return spOv_priority;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
+			return spOv_gravityPoint;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+			return spOv_gravityPointDistanceScale;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			return spOv_gravityPointAttenuation;
+		default:
+			print_line("The Bullet areas dosn't suppot this param: " + itos(p_param));
+			return Variant();
+	}
+}
+
+void AreaBullet::set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method) {
+	InOutEventCallback &ev = eventsCallbacks[static_cast<int>(p_callbackObjectType)];
+	ev.event_callback_id = p_id;
+	ev.event_callback_method = p_method;
+
+	/// Set if monitoring
+	if (eventsCallbacks[0].event_callback_id || eventsCallbacks[1].event_callback_id) {
+		set_godot_object_flags(get_godot_object_flags() | GOF_IS_MONITORING_AREA);
+	} else {
+		set_godot_object_flags(get_godot_object_flags() & (~GOF_IS_MONITORING_AREA));
+	}
+}
+
+bool AreaBullet::has_event_callback(Type p_callbackObjectType) {
+	return eventsCallbacks[static_cast<int>(p_callbackObjectType)].event_callback_id;
+}
+
+void AreaBullet::on_enter_area(AreaBullet *p_area) {
+}
+
+void AreaBullet::on_exit_area(AreaBullet *p_area) {
+	CollisionObjectBullet::on_exit_area(p_area);
+}

modules/bullet/area_bullet.h

@@ -0,0 +1,169 @@
+/*************************************************************************/
+/*  area_bullet.h                                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef AREABULLET_H
+#define AREABULLET_H
+
+#include "collision_object_bullet.h"
+#include "core/vector.h"
+#include "servers/physics_server.h"
+#include "space_bullet.h"
+
+class btGhostObject;
+
+class AreaBullet : public RigidCollisionObjectBullet {
+	friend void SpaceBullet::check_ghost_overlaps();
+
+public:
+	struct InOutEventCallback {
+		ObjectID event_callback_id;
+		StringName event_callback_method;
+
+		InOutEventCallback()
+			: event_callback_id(0) {}
+	};
+
+	enum OverlapState {
+		OVERLAP_STATE_DIRTY = 0, // Mark processed overlaps
+		OVERLAP_STATE_INSIDE, // Mark old overlap
+		OVERLAP_STATE_ENTER, // Mark just enter overlap
+		OVERLAP_STATE_EXIT // Mark ended overlaps
+	};
+
+	struct OverlappingObjectData {
+		CollisionObjectBullet *object;
+		OverlapState state;
+
+		OverlappingObjectData()
+			: object(NULL), state(OVERLAP_STATE_ENTER) {}
+		OverlappingObjectData(CollisionObjectBullet *p_object, OverlapState p_state)
+			: object(p_object), state(p_state) {}
+		OverlappingObjectData(const OverlappingObjectData &other) {
+			operator=(other);
+		}
+		void operator=(const OverlappingObjectData &other) {
+			object = other.object;
+			state = other.state;
+		}
+	};
+
+private:
+	// These are used by function callEvent. Instead to create this each call I create if one time.
+	Variant call_event_res[5];
+	Variant *call_event_res_ptr[5];
+
+	btGhostObject *btGhost;
+	Vector<OverlappingObjectData> overlappingObjects;
+	bool monitorable;
+
+	PhysicsServer::AreaSpaceOverrideMode spOv_mode;
+	bool spOv_gravityPoint;
+	real_t spOv_gravityPointDistanceScale;
+	real_t spOv_gravityPointAttenuation;
+	Vector3 spOv_gravityVec;
+	real_t spOv_gravityMag;
+	real_t spOv_linearDump;
+	real_t spOv_angularDump;
+	int spOv_priority;
+
+	bool isScratched;
+
+	InOutEventCallback eventsCallbacks[2];
+
+public:
+	AreaBullet();
+	~AreaBullet();
+
+	_FORCE_INLINE_ btGhostObject *get_bt_ghost() const { return btGhost; }
+	int find_overlapping_object(CollisionObjectBullet *p_colObj);
+
+	void set_monitorable(bool p_monitorable);
+	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
+
+	bool is_monitoring() const;
+
+	_FORCE_INLINE_ void set_spOv_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { spOv_mode = p_mode; }
+	_FORCE_INLINE_ PhysicsServer::AreaSpaceOverrideMode get_spOv_mode() { return spOv_mode; }
+
+	_FORCE_INLINE_ void set_spOv_gravityPoint(bool p_isGP) { spOv_gravityPoint = p_isGP; }
+	_FORCE_INLINE_ bool is_spOv_gravityPoint() { return spOv_gravityPoint; }
+
+	_FORCE_INLINE_ void set_spOv_gravityPointDistanceScale(real_t p_GPDS) { spOv_gravityPointDistanceScale = p_GPDS; }
+	_FORCE_INLINE_ real_t get_spOv_gravityPointDistanceScale() { return spOv_gravityPointDistanceScale; }
+
+	_FORCE_INLINE_ void set_spOv_gravityPointAttenuation(real_t p_GPA) { spOv_gravityPointAttenuation = p_GPA; }
+	_FORCE_INLINE_ real_t get_spOv_gravityPointAttenuation() { return spOv_gravityPointAttenuation; }
+
+	_FORCE_INLINE_ void set_spOv_gravityVec(Vector3 p_vec) { spOv_gravityVec = p_vec; }
+	_FORCE_INLINE_ const Vector3 &get_spOv_gravityVec() const { return spOv_gravityVec; }
+
+	_FORCE_INLINE_ void set_spOv_gravityMag(real_t p_gravityMag) { spOv_gravityMag = p_gravityMag; }
+	_FORCE_INLINE_ real_t get_spOv_gravityMag() { return spOv_gravityMag; }
+
+	_FORCE_INLINE_ void set_spOv_linearDump(real_t p_linearDump) { spOv_linearDump = p_linearDump; }
+	_FORCE_INLINE_ real_t get_spOv_linearDamp() { return spOv_linearDump; }
+
+	_FORCE_INLINE_ void set_spOv_angularDump(real_t p_angularDump) { spOv_angularDump = p_angularDump; }
+	_FORCE_INLINE_ real_t get_spOv_angularDamp() { return spOv_angularDump; }
+
+	_FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; }
+	_FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; }
+
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
+
+	virtual void dispatch_callbacks();
+	void call_event(CollisionObjectBullet *p_otherObject, PhysicsServer::AreaBodyStatus p_status);
+	void set_on_state_change(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
+	void scratch();
+
+	void remove_all_overlapping_instantly();
+	// Dispatch the callbacks and removes from overlapping list
+	void remove_overlapping_instantly(CollisionObjectBullet *p_object);
+
+	virtual void on_collision_filters_change();
+	virtual void on_collision_checker_start() {}
+
+	void add_overlap(CollisionObjectBullet *p_otherObject);
+	void put_overlap_as_exit(int p_index);
+	void put_overlap_as_inside(int p_index);
+
+	void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer::AreaParameter p_param) const;
+
+	void set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method);
+	bool has_event_callback(Type p_callbackObjectType);
+
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
+};
+
+#endif

modules/bullet/btRayShape.cpp

@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  btRayShape.h                                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "btRayShape.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "math/math_funcs.h"
+
+btRayShape::btRayShape(btScalar length)
+	: btConvexInternalShape(),
+	  m_shapeAxis(0, 0, 1) {
+	m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE;
+	setLength(length);
+}
+
+btRayShape::~btRayShape() {
+}
+
+void btRayShape::setLength(btScalar p_length) {
+
+	m_length = p_length;
+	reload_cache();
+}
+
+btVector3 btRayShape::localGetSupportingVertex(const btVector3 &vec) const {
+	return localGetSupportingVertexWithoutMargin(vec) + (m_shapeAxis * m_collisionMargin);
+}
+
+btVector3 btRayShape::localGetSupportingVertexWithoutMargin(const btVector3 &vec) const {
+	if (vec.z() > 0)
+		return m_shapeAxis * m_cacheScaledLength;
+	else
+		return btVector3(0, 0, 0);
+}
+
+void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const {
+	for (int i = 0; i < numVectors; ++i) {
+		supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]);
+	}
+}
+
+void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const {
+#define MARGIN_BROADPHASE 0.1
+	btVector3 localAabbMin(0, 0, 0);
+	btVector3 localAabbMax(m_shapeAxis * m_length);
+	btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
+}
+
+void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
+	inertia.setZero();
+}
+
+int btRayShape::getNumPreferredPenetrationDirections() const {
+	return 0;
+}
+
+void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const {
+	penetrationVector.setZero();
+}
+
+void btRayShape::reload_cache() {
+
+	m_cacheScaledLength = m_length * m_localScaling[2] + m_collisionMargin;
+
+	m_cacheSupportPoint.setIdentity();
+	m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
+}

modules/bullet/btRayShape.h

@@ -0,0 +1,87 @@
+/*************************************************************************/
+/*  btRayShape.h                                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+/// IMPORTANT The class name and filename was created by following Bullet writing rules for an easy (eventually ) porting to bullet
+/// This shape is a custom shape that is not present to Bullet physics engine
+#ifndef BTRAYSHAPE_H
+#define BTRAYSHAPE_H
+
+#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
+
+/// Ray shape around z axis
+ATTRIBUTE_ALIGNED16(class)
+btRayShape : public btConvexInternalShape {
+
+	btScalar m_length;
+	/// The default axis is the z
+	btVector3 m_shapeAxis;
+
+	btTransform m_cacheSupportPoint;
+	btScalar m_cacheScaledLength;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btRayShape(btScalar length);
+	virtual ~btRayShape();
+
+	void setLength(btScalar p_length);
+	btScalar getLength() const { return m_length; }
+
+	const btTransform &getSupportPoint() const { return m_cacheSupportPoint; }
+	const btScalar &getScaledLength() const { return m_cacheScaledLength; }
+
+	virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const;
+#ifndef __SPU__
+	virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const;
+#endif //#ifndef __SPU__
+
+	virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const;
+
+	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+	virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const;
+
+#ifndef __SPU__
+	virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const;
+
+	virtual const char *getName() const {
+		return "RayZ";
+	}
+#endif //__SPU__
+
+	virtual int getNumPreferredPenetrationDirections() const;
+	virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const;
+
+private:
+	void reload_cache();
+};
+
+#endif // BTRAYSHAPE_H

modules/bullet/bullet_physics_server.cpp

@@ -0,0 +1,1339 @@
+/*************************************************************************/
+/*  bullet_physics_server.cpp                                            */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "bullet_physics_server.h"
+#include "LinearMath/btVector3.h"
+#include "bullet_utilities.h"
+#include "class_db.h"
+#include "cone_twist_joint_bullet.h"
+#include "core/error_macros.h"
+#include "core/ustring.h"
+#include "generic_6dof_joint_bullet.h"
+#include "hinge_joint_bullet.h"
+#include "pin_joint_bullet.h"
+#include "shape_bullet.h"
+#include "slider_joint_bullet.h"
+#include <assert.h>
+
+#define CreateThenReturnRID(owner, ridData) \
+	RID rid = owner.make_rid(ridData);      \
+	ridData->set_self(rid);                 \
+	ridData->_set_physics_server(this);     \
+	return rid;
+
+// <--------------- Joint creation asserts
+/// Assert the body is assigned to a space
+#define JointAssertSpace(body, bIndex, ret)                                                           \
+	if (!body->get_space()) {                                                                         \
+		ERR_PRINTS("Before create a joint the Body" + String(bIndex) + " must be added to a space!"); \
+		return ret;                                                                                   \
+	}
+
+/// Assert the two bodies of joint are in the same space
+#define JointAssertSameSpace(bodyA, bodyB, ret)                                                   \
+	if (bodyA->get_space() != bodyB->get_space()) {                                               \
+		ERR_PRINT("In order to create a joint the Body_A and Body_B must be in the same space!"); \
+		return RID();                                                                             \
+	}
+
+#define AddJointToSpace(body, joint, disableCollisionsBetweenLinkedBodies) \
+	body->get_space()->add_constraint(joint, disableCollisionsBetweenLinkedBodies);
+// <--------------- Joint creation asserts
+
+btEmptyShape *BulletPhysicsServer::emptyShape(ShapeBullet::create_shape_empty());
+
+btEmptyShape *BulletPhysicsServer::get_empty_shape() {
+	return emptyShape;
+}
+
+void BulletPhysicsServer::_bind_methods() {
+	//ClassDB::bind_method(D_METHOD("DoTest"), &BulletPhysicsServer::DoTest);
+}
+
+BulletPhysicsServer::BulletPhysicsServer()
+	: PhysicsServer(),
+	  active(true),
+	  activeSpace(NULL) {}
+
+BulletPhysicsServer::~BulletPhysicsServer() {}
+
+RID BulletPhysicsServer::shape_create(ShapeType p_shape) {
+	ShapeBullet *shape = NULL;
+
+	switch (p_shape) {
+		case SHAPE_PLANE: {
+
+			shape = bulletnew(PlaneShapeBullet);
+		} break;
+		case SHAPE_SPHERE: {
+
+			shape = bulletnew(SphereShapeBullet);
+		} break;
+		case SHAPE_BOX: {
+
+			shape = bulletnew(BoxShapeBullet);
+		} break;
+		case SHAPE_CAPSULE: {
+
+			shape = bulletnew(CapsuleShapeBullet);
+		} break;
+		case SHAPE_CONVEX_POLYGON: {
+
+			shape = bulletnew(ConvexPolygonShapeBullet);
+		} break;
+		case SHAPE_CONCAVE_POLYGON: {
+
+			shape = bulletnew(ConcavePolygonShapeBullet);
+		} break;
+		case SHAPE_HEIGHTMAP: {
+
+			shape = bulletnew(HeightMapShapeBullet);
+		} break;
+		case SHAPE_RAY: {
+			shape = bulletnew(RayShapeBullet);
+		} break;
+		case SHAPE_CUSTOM:
+		defaul:
+			ERR_FAIL_V(RID());
+			break;
+	}
+
+	CreateThenReturnRID(shape_owner, shape)
+}
+
+void BulletPhysicsServer::shape_set_data(RID p_shape, const Variant &p_data) {
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_data(p_data);
+}
+
+void BulletPhysicsServer::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+	//WARN_PRINT("Bias not supported by Bullet physics engine");
+}
+
+PhysicsServer::ShapeType BulletPhysicsServer::shape_get_type(RID p_shape) const {
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, PhysicsServer::SHAPE_CUSTOM);
+	return shape->get_type();
+}
+
+Variant BulletPhysicsServer::shape_get_data(RID p_shape) const {
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, Variant());
+	return shape->get_data();
+}
+
+real_t BulletPhysicsServer::shape_get_custom_solver_bias(RID p_shape) const {
+	//WARN_PRINT("Bias not supported by Bullet physics engine");
+	return 0.;
+}
+
+RID BulletPhysicsServer::space_create() {
+	SpaceBullet *space = bulletnew(SpaceBullet(false));
+	CreateThenReturnRID(space_owner, space);
+}
+
+void BulletPhysicsServer::space_set_active(RID p_space, bool p_active) {
+	if (p_active) {
+		if (activeSpace) {
+			// There is another space and this cannot be activated
+			ERR_PRINT("There is another space, before activate new one deactivate the current space.");
+		} else {
+			SpaceBullet *space = space_owner.get(p_space);
+			if (space) {
+				activeSpace = space;
+			} else {
+				ERR_PRINT("The passed RID is not a valid space. Please provide a RID with SpaceBullet type.");
+			}
+		}
+	} else {
+		if (!space_is_active(p_space)) {
+			activeSpace = NULL;
+		}
+	}
+}
+
+bool BulletPhysicsServer::space_is_active(RID p_space) const {
+	return NULL != activeSpace && activeSpace == p_space.get_data();
+}
+
+void BulletPhysicsServer::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	space->set_param(p_param, p_value);
+}
+
+real_t BulletPhysicsServer::space_get_param(RID p_space, SpaceParameter p_param) const {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space, 0);
+	return space->get_param(p_param);
+}
+
+PhysicsDirectSpaceState *BulletPhysicsServer::space_get_direct_state(RID p_space) {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space, NULL);
+
+	return space->get_direct_state();
+}
+
+void BulletPhysicsServer::space_set_debug_contacts(RID p_space, int p_max_contacts) {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+
+	space->set_debug_contacts(p_max_contacts);
+}
+
+Vector<Vector3> BulletPhysicsServer::space_get_contacts(RID p_space) const {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space, Vector<Vector3>());
+
+	return space->get_debug_contacts();
+}
+
+int BulletPhysicsServer::space_get_contact_count(RID p_space) const {
+	SpaceBullet *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space, 0);
+
+	return space->get_debug_contact_count();
+}
+
+RID BulletPhysicsServer::area_create() {
+	AreaBullet *area = bulletnew(AreaBullet);
+	area->set_collision_layer(1);
+	area->set_collision_mask(1);
+	CreateThenReturnRID(area_owner, area)
+}
+
+void BulletPhysicsServer::area_set_space(RID p_area, RID p_space) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	SpaceBullet *space = NULL;
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+	area->set_space(space);
+}
+
+RID BulletPhysicsServer::area_get_space(RID p_area) const {
+	AreaBullet *area = area_owner.get(p_area);
+	return area->get_space()->get_self();
+}
+
+void BulletPhysicsServer::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area)
+
+	area->set_spOv_mode(p_mode);
+}
+
+PhysicsServer::AreaSpaceOverrideMode BulletPhysicsServer::area_get_space_override_mode(RID p_area) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED);
+
+	return area->get_spOv_mode();
+}
+
+void BulletPhysicsServer::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	area->add_shape(shape, p_transform);
+}
+
+void BulletPhysicsServer::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	area->set_shape(p_shape_idx, shape);
+}
+
+void BulletPhysicsServer::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_shape_transform(p_shape_idx, p_transform);
+}
+
+int BulletPhysicsServer::area_get_shape_count(RID p_area) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, 0);
+
+	return area->get_shape_count();
+}
+
+RID BulletPhysicsServer::area_get_shape(RID p_area, int p_shape_idx) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, RID());
+
+	return area->get_shape(p_shape_idx)->get_self();
+}
+
+Transform BulletPhysicsServer::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, Transform());
+
+	return area->get_shape_transform(p_shape_idx);
+}
+
+void BulletPhysicsServer::area_remove_shape(RID p_area, int p_shape_idx) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	return area->remove_shape(p_shape_idx);
+}
+
+void BulletPhysicsServer::area_clear_shapes(RID p_area) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	for (int i = area->get_shape_count(); 0 < i; --i)
+		area->remove_shape(0);
+}
+
+void BulletPhysicsServer::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void BulletPhysicsServer::area_attach_object_instance_id(RID p_area, ObjectID p_ID) {
+	if (space_owner.owns(p_area)) {
+		return;
+	}
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_instance_id(p_ID);
+}
+
+ObjectID BulletPhysicsServer::area_get_object_instance_id(RID p_area) const {
+	if (space_owner.owns(p_area)) {
+		return 0;
+	}
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, ObjectID());
+	return area->get_instance_id();
+}
+
+void BulletPhysicsServer::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
+	if (space_owner.owns(p_area)) {
+		SpaceBullet *space = space_owner.get(p_area);
+		if (space) {
+			space->set_param(p_param, p_value);
+		}
+	} else {
+
+		AreaBullet *area = area_owner.get(p_area);
+		ERR_FAIL_COND(!area);
+
+		area->set_param(p_param, p_value);
+	}
+}
+
+Variant BulletPhysicsServer::area_get_param(RID p_area, AreaParameter p_param) const {
+	if (space_owner.owns(p_area)) {
+		SpaceBullet *space = space_owner.get(p_area);
+		return space->get_param(p_param);
+	} else {
+		AreaBullet *area = area_owner.get(p_area);
+		ERR_FAIL_COND_V(!area, Variant());
+
+		return area->get_param(p_param);
+	}
+}
+
+void BulletPhysicsServer::area_set_transform(RID p_area, const Transform &p_transform) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_transform(p_transform);
+}
+
+Transform BulletPhysicsServer::area_get_transform(RID p_area) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, Transform());
+	return area->get_transform();
+}
+
+void BulletPhysicsServer::area_set_collision_mask(RID p_area, uint32_t p_mask) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_collision_mask(p_mask);
+}
+
+void BulletPhysicsServer::area_set_collision_layer(RID p_area, uint32_t p_layer) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_collision_layer(p_layer);
+}
+
+void BulletPhysicsServer::area_set_monitorable(RID p_area, bool p_monitorable) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitorable(p_monitorable);
+}
+
+void BulletPhysicsServer::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_event_callback(CollisionObjectBullet::TYPE_RIGID_BODY, p_receiver ? p_receiver->get_instance_id() : 0, p_method);
+}
+
+void BulletPhysicsServer::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_event_callback(CollisionObjectBullet::TYPE_AREA, p_receiver ? p_receiver->get_instance_id() : 0, p_method);
+}
+
+void BulletPhysicsServer::area_set_ray_pickable(RID p_area, bool p_enable) {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_ray_pickable(p_enable);
+}
+
+bool BulletPhysicsServer::area_is_ray_pickable(RID p_area) const {
+	AreaBullet *area = area_owner.get(p_area);
+	ERR_FAIL_COND_V(!area, false);
+	return area->is_ray_pickable();
+}
+
+RID BulletPhysicsServer::body_create(BodyMode p_mode, bool p_init_sleeping) {
+	RigidBodyBullet *body = bulletnew(RigidBodyBullet);
+	body->set_mode(p_mode);
+	body->set_collision_layer(1);
+	body->set_collision_mask(1);
+	if (p_init_sleeping)
+		body->set_state(BODY_STATE_SLEEPING, p_init_sleeping);
+	CreateThenReturnRID(rigid_body_owner, body);
+}
+
+void BulletPhysicsServer::body_set_space(RID p_body, RID p_space) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	SpaceBullet *space = NULL;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (body->get_space() == space)
+		return; //pointles
+
+	body->set_space(space);
+}
+
+RID BulletPhysicsServer::body_get_space(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, RID());
+
+	SpaceBullet *space = body->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+}
+
+void BulletPhysicsServer::body_set_mode(RID p_body, PhysicsServer::BodyMode p_mode) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_mode(p_mode);
+}
+
+PhysicsServer::BodyMode BulletPhysicsServer::body_get_mode(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
+	return body->get_mode();
+}
+
+void BulletPhysicsServer::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform) {
+
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	body->add_shape(shape, p_transform);
+}
+
+void BulletPhysicsServer::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	ShapeBullet *shape = shape_owner.get(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	body->set_shape(p_shape_idx, shape);
+}
+
+void BulletPhysicsServer::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_shape_transform(p_shape_idx, p_transform);
+}
+
+int BulletPhysicsServer::body_get_shape_count(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+	return body->get_shape_count();
+}
+
+RID BulletPhysicsServer::body_get_shape(RID p_body, int p_shape_idx) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, RID());
+
+	ShapeBullet *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape, RID());
+
+	return shape->get_self();
+}
+
+Transform BulletPhysicsServer::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Transform());
+	return body->get_shape_transform(p_shape_idx);
+}
+
+void BulletPhysicsServer::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void BulletPhysicsServer::body_remove_shape(RID p_body, int p_shape_idx) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void BulletPhysicsServer::body_clear_shapes(RID p_body) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_all_shapes();
+}
+
+void BulletPhysicsServer::body_attach_object_instance_id(RID p_body, uint32_t p_ID) {
+	CollisionObjectBullet *body = get_collisin_object(p_body);
+	if (!body) {
+		body = soft_body_owner.get(p_body);
+	}
+	ERR_FAIL_COND(!body);
+
+	body->set_instance_id(p_ID);
+}
+
+uint32_t BulletPhysicsServer::body_get_object_instance_id(RID p_body) const {
+	CollisionObjectBullet *body = get_collisin_object(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_instance_id();
+}
+
+void BulletPhysicsServer::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_continuous_collision_detection(p_enable);
+}
+
+bool BulletPhysicsServer::body_is_continuous_collision_detection_enabled(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+
+	return body->is_continuous_collision_detection_enabled();
+}
+
+void BulletPhysicsServer::body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t BulletPhysicsServer::body_get_collision_layer(RID p_body) const {
+	const RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_layer();
+}
+
+void BulletPhysicsServer::body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t BulletPhysicsServer::body_get_collision_mask(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_mask();
+}
+
+void BulletPhysicsServer::body_set_user_flags(RID p_body, uint32_t p_flags) {
+	WARN_PRINT("This function si not currently supported by bullet and Godot");
+}
+
+uint32_t BulletPhysicsServer::body_get_user_flags(RID p_body) const {
+	WARN_PRINT("This function si not currently supported by bullet and Godot");
+	return 0;
+}
+
+void BulletPhysicsServer::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_param(p_param, p_value);
+}
+
+float BulletPhysicsServer::body_get_param(RID p_body, BodyParameter p_param) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_param(p_param);
+}
+
+void BulletPhysicsServer::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_state(p_state, p_variant);
+}
+
+Variant BulletPhysicsServer::body_get_state(RID p_body, BodyState p_state) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Variant());
+
+	return body->get_state(p_state);
+}
+
+void BulletPhysicsServer::body_set_applied_force(RID p_body, const Vector3 &p_force) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_force(p_force);
+}
+
+Vector3 BulletPhysicsServer::body_get_applied_force(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+	return body->get_applied_force();
+}
+
+void BulletPhysicsServer::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_torque(p_torque);
+}
+
+Vector3 BulletPhysicsServer::body_get_applied_torque(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+
+	return body->get_applied_torque();
+}
+
+void BulletPhysicsServer::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_impulse(p_pos, p_impulse);
+}
+
+void BulletPhysicsServer::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_torque_impulse(p_impulse);
+}
+
+void BulletPhysicsServer::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	Vector3 v = body->get_linear_velocity();
+	Vector3 axis = p_axis_velocity.normalized();
+	v -= axis * axis.dot(v);
+	v += p_axis_velocity;
+	body->set_linear_velocity(v);
+}
+
+void BulletPhysicsServer::body_set_axis_lock(RID p_body, PhysicsServer::BodyAxisLock p_lock) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_axis_lock(p_lock);
+}
+
+PhysicsServer::BodyAxisLock BulletPhysicsServer::body_get_axis_lock(RID p_body) const {
+	const RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, BODY_AXIS_LOCK_DISABLED);
+	return body->get_axis_lock();
+}
+
+void BulletPhysicsServer::body_add_collision_exception(RID p_body, RID p_body_b) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	RigidBodyBullet *other_body = rigid_body_owner.get(p_body_b);
+	ERR_FAIL_COND(!other_body);
+
+	body->add_collision_exception(other_body);
+}
+
+void BulletPhysicsServer::body_remove_collision_exception(RID p_body, RID p_body_b) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	RigidBodyBullet *other_body = rigid_body_owner.get(p_body_b);
+	ERR_FAIL_COND(!other_body);
+
+	body->remove_collision_exception(other_body);
+}
+
+void BulletPhysicsServer::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+}
+
+void BulletPhysicsServer::body_set_max_contacts_reported(RID p_body, int p_contacts) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_max_collisions_detection(p_contacts);
+}
+
+int BulletPhysicsServer::body_get_max_contacts_reported(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_max_collisions_detection();
+}
+
+void BulletPhysicsServer::body_set_contacts_reported_depth_threshold(RID p_body, float p_treshold) {
+	WARN_PRINT("Not supported by bullet and even Godot");
+}
+
+float BulletPhysicsServer::body_get_contacts_reported_depth_threshold(RID p_body) const {
+	WARN_PRINT("Not supported by bullet and even Godot");
+	return 0.;
+}
+
+void BulletPhysicsServer::body_set_omit_force_integration(RID p_body, bool p_omit) {
+	WARN_PRINT("Not supported by bullet");
+}
+
+bool BulletPhysicsServer::body_is_omitting_force_integration(RID p_body) const {
+	WARN_PRINT("Not supported by bullet");
+	return false;
+}
+
+void BulletPhysicsServer::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_force_integration_callback(p_receiver->get_instance_id(), p_method, p_udata);
+}
+
+void BulletPhysicsServer::body_set_ray_pickable(RID p_body, bool p_enable) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_ray_pickable(p_enable);
+}
+
+bool BulletPhysicsServer::body_is_ray_pickable(RID p_body) const {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	return body->is_ray_pickable();
+}
+
+PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, NULL);
+	return BulletPhysicsDirectBodyState::get_singleton(body);
+}
+
+bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) {
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
+}
+
+RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {
+	SoftBodyBullet *body = bulletnew(SoftBodyBullet);
+	body->set_collision_layer(1);
+	body->set_collision_mask(1);
+	if (p_init_sleeping)
+		body->set_activation_state(false);
+	CreateThenReturnRID(soft_body_owner, body);
+}
+
+void BulletPhysicsServer::soft_body_set_space(RID p_body, RID p_space) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	SpaceBullet *space = NULL;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (body->get_space() == space)
+		return; //pointles
+
+	body->set_space(space);
+}
+
+RID BulletPhysicsServer::soft_body_get_space(RID p_body) const {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, RID());
+
+	SpaceBullet *space = body->get_space();
+	if (!space)
+		return RID();
+	return space->get_self();
+}
+
+void BulletPhysicsServer::soft_body_set_trimesh_body_shape(RID p_body, PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_trimesh_body_shape(p_indices, p_vertices, p_triangles_num);
+}
+
+void BulletPhysicsServer::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t BulletPhysicsServer::soft_body_get_collision_layer(RID p_body) const {
+	const SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_layer();
+}
+
+void BulletPhysicsServer::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t BulletPhysicsServer::soft_body_get_collision_mask(RID p_body) const {
+	const SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_mask();
+}
+
+void BulletPhysicsServer::soft_body_add_collision_exception(RID p_body, RID p_body_b) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	CollisionObjectBullet *other_body = rigid_body_owner.get(p_body_b);
+	if (!other_body) {
+		other_body = soft_body_owner.get(p_body_b);
+	}
+	ERR_FAIL_COND(!other_body);
+
+	body->add_collision_exception(other_body);
+}
+
+void BulletPhysicsServer::soft_body_remove_collision_exception(RID p_body, RID p_body_b) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	CollisionObjectBullet *other_body = rigid_body_owner.get(p_body_b);
+	if (!other_body) {
+		other_body = soft_body_owner.get(p_body_b);
+	}
+	ERR_FAIL_COND(!other_body);
+
+	body->remove_collision_exception(other_body);
+}
+
+void BulletPhysicsServer::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+}
+
+void BulletPhysicsServer::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
+	print_line("TODO MUST BE IMPLEMENTED");
+}
+
+Variant BulletPhysicsServer::soft_body_get_state(RID p_body, BodyState p_state) const {
+	print_line("TODO MUST BE IMPLEMENTED");
+	return Variant();
+}
+
+void BulletPhysicsServer::soft_body_set_transform(RID p_body, const Transform &p_transform) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_transform(p_transform);
+}
+
+Transform BulletPhysicsServer::soft_body_get_transform(RID p_body) const {
+	const SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Transform());
+
+	return body->get_transform();
+}
+
+void BulletPhysicsServer::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_ray_pickable(p_enable);
+}
+
+bool BulletPhysicsServer::soft_body_is_ray_pickable(RID p_body) const {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	return body->is_ray_pickable();
+}
+
+PhysicsServer::JointType BulletPhysicsServer::joint_get_type(RID p_joint) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, JOINT_PIN);
+	return joint->get_type();
+}
+
+void BulletPhysicsServer::joint_set_solver_priority(RID p_joint, int p_priority) {
+	//WARN_PRINTS("Joint priority not supported by bullet");
+}
+
+int BulletPhysicsServer::joint_get_solver_priority(RID p_joint) const {
+	//WARN_PRINTS("Joint priority not supported by bullet");
+	return 0;
+}
+
+RID BulletPhysicsServer::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	ERR_FAIL_COND_V(body_A == body_B, RID());
+
+	JointBullet *joint = bulletnew(PinJointBullet(body_A, p_local_A, body_B, p_local_B));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+void BulletPhysicsServer::pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	pin_joint->set_param(p_param, p_value);
+}
+
+float BulletPhysicsServer::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0);
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	return pin_joint->get_param(p_param);
+}
+
+void BulletPhysicsServer::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	pin_joint->setPivotInA(p_A);
+}
+
+Vector3 BulletPhysicsServer::pin_joint_get_local_a(RID p_joint) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	return pin_joint->getPivotInA();
+}
+
+void BulletPhysicsServer::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	pin_joint->setPivotInB(p_B);
+}
+
+Vector3 BulletPhysicsServer::pin_joint_get_local_b(RID p_joint) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	PinJointBullet *pin_joint = static_cast<PinJointBullet *>(joint);
+	return pin_joint->getPivotInB();
+}
+
+RID BulletPhysicsServer::joint_create_hinge(RID p_body_A, const Transform &p_hinge_A, RID p_body_B, const Transform &p_hinge_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	ERR_FAIL_COND_V(body_A == body_B, RID());
+
+	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_hinge_A, p_hinge_B));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+RID BulletPhysicsServer::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	ERR_FAIL_COND_V(body_A == body_B, RID());
+
+	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+void BulletPhysicsServer::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
+	hinge_joint->set_param(p_param, p_value);
+}
+
+float BulletPhysicsServer::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0);
+	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
+	return hinge_joint->get_param(p_param);
+}
+
+void BulletPhysicsServer::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
+	hinge_joint->set_flag(p_flag, p_value);
+}
+
+bool BulletPhysicsServer::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false);
+	HingeJointBullet *hinge_joint = static_cast<HingeJointBullet *>(joint);
+	return hinge_joint->get_flag(p_flag);
+}
+
+RID BulletPhysicsServer::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	ERR_FAIL_COND_V(body_A == body_B, RID());
+
+	JointBullet *joint = bulletnew(SliderJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+void BulletPhysicsServer::slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER);
+	SliderJointBullet *slider_joint = static_cast<SliderJointBullet *>(joint);
+	slider_joint->set_param(p_param, p_value);
+}
+
+float BulletPhysicsServer::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_SLIDER, 0);
+	SliderJointBullet *slider_joint = static_cast<SliderJointBullet *>(joint);
+	return slider_joint->get_param(p_param);
+}
+
+RID BulletPhysicsServer::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	JointBullet *joint = bulletnew(ConeTwistJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+void BulletPhysicsServer::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST);
+	ConeTwistJointBullet *coneTwist_joint = static_cast<ConeTwistJointBullet *>(joint);
+	coneTwist_joint->set_param(p_param, p_value);
+}
+
+float BulletPhysicsServer::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0.);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0.);
+	ConeTwistJointBullet *coneTwist_joint = static_cast<ConeTwistJointBullet *>(joint);
+	return coneTwist_joint->get_param(p_param);
+}
+
+RID BulletPhysicsServer::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
+	ERR_FAIL_COND_V(!body_A, RID());
+	JointAssertSpace(body_A, "A", RID());
+
+	RigidBodyBullet *body_B = NULL;
+	if (p_body_B.is_valid()) {
+		body_B = rigid_body_owner.get(p_body_B);
+		JointAssertSpace(body_B, "B", RID());
+		JointAssertSameSpace(body_A, body_B, RID());
+	}
+
+	ERR_FAIL_COND_V(body_A == body_B, RID());
+
+	JointBullet *joint = bulletnew(Generic6DOFJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B, true));
+	AddJointToSpace(body_A, joint, true);
+
+	CreateThenReturnRID(joint_owner, joint);
+}
+
+void BulletPhysicsServer::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, float p_value) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	generic_6dof_joint->set_param(p_axis, p_param, p_value);
+}
+
+float BulletPhysicsServer::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	return generic_6dof_joint->get_param(p_axis, p_param);
+}
+
+void BulletPhysicsServer::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	generic_6dof_joint->set_flag(p_axis, p_flag, p_enable);
+}
+
+bool BulletPhysicsServer::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false);
+	Generic6DOFJointBullet *generic_6dof_joint = static_cast<Generic6DOFJointBullet *>(joint);
+	return generic_6dof_joint->get_flag(p_axis, p_flag);
+}
+
+void BulletPhysicsServer::free(RID p_rid) {
+	if (shape_owner.owns(p_rid)) {
+
+		ShapeBullet *shape = shape_owner.get(p_rid);
+
+		// Notify the shape is configured
+		for (Map<ShapeOwnerBullet *, int>::Element *element = shape->get_owners().front(); element; element = element->next()) {
+			static_cast<ShapeOwnerBullet *>(element->key())->remove_shape(shape);
+		}
+
+		shape_owner.free(p_rid);
+		bulletdelete(shape);
+	} else if (rigid_body_owner.owns(p_rid)) {
+
+		RigidBodyBullet *body = rigid_body_owner.get(p_rid);
+
+		body->set_space(NULL);
+
+		body->remove_all_shapes(true);
+
+		rigid_body_owner.free(p_rid);
+		bulletdelete(body);
+
+	} else if (soft_body_owner.owns(p_rid)) {
+
+		SoftBodyBullet *body = soft_body_owner.get(p_rid);
+
+		body->set_space(NULL);
+
+		soft_body_owner.free(p_rid);
+		bulletdelete(body);
+
+	} else if (area_owner.owns(p_rid)) {
+
+		AreaBullet *area = area_owner.get(p_rid);
+
+		area->set_space(NULL);
+
+		area->remove_all_shapes(true);
+
+		area_owner.free(p_rid);
+		bulletdelete(area);
+
+	} else if (joint_owner.owns(p_rid)) {
+
+		JointBullet *joint = joint_owner.get(p_rid);
+		joint->destroy_internal_constraint();
+		joint_owner.free(p_rid);
+		bulletdelete(joint);
+
+	} else if (space_owner.owns(p_rid)) {
+
+		SpaceBullet *space = space_owner.get(p_rid);
+
+		space->remove_all_collision_objects();
+
+		space_set_active(p_rid, false);
+		space_owner.free(p_rid);
+		bulletdelete(space);
+	} else {
+
+		ERR_EXPLAIN("Invalid ID");
+		ERR_FAIL();
+	}
+}
+
+void BulletPhysicsServer::init() {
+	BulletPhysicsDirectBodyState::initSingleton();
+}
+
+void BulletPhysicsServer::step(float p_deltaTime) {
+	if (!active)
+		return;
+
+	BulletPhysicsDirectBodyState::singleton_setDeltaTime(p_deltaTime);
+	if (activeSpace) {
+		activeSpace->step(p_deltaTime);
+	}
+}
+
+void BulletPhysicsServer::sync() {
+}
+
+void BulletPhysicsServer::flush_queries() {
+}
+
+void BulletPhysicsServer::finish() {
+	BulletPhysicsDirectBodyState::destroySingleton();
+}
+
+int BulletPhysicsServer::get_process_info(ProcessInfo p_info) {
+	return 0;
+}
+
+CollisionObjectBullet *BulletPhysicsServer::get_collisin_object(RID p_object) const {
+	if (rigid_body_owner.owns(p_object)) {
+		return rigid_body_owner.getornull(p_object);
+	}
+	if (area_owner.owns(p_object)) {
+		return area_owner.getornull(p_object);
+	}
+	if (soft_body_owner.owns(p_object)) {
+		return soft_body_owner.getornull(p_object);
+	}
+	return NULL;
+}
+
+RigidCollisionObjectBullet *BulletPhysicsServer::get_rigid_collisin_object(RID p_object) const {
+	if (rigid_body_owner.owns(p_object)) {
+		return rigid_body_owner.getornull(p_object);
+	}
+	if (area_owner.owns(p_object)) {
+		return area_owner.getornull(p_object);
+	}
+	return NULL;
+}

modules/bullet/bullet_physics_server.h

@@ -0,0 +1,358 @@
+/*************************************************************************/
+/*  bullet_physics_server.h                                              */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BULLET_PHYSICS_SERVER_H
+#define BULLET_PHYSICS_SERVER_H
+
+#include "area_bullet.h"
+#include "joint_bullet.h"
+#include "rid.h"
+#include "rigid_body_bullet.h"
+#include "servers/physics_server.h"
+#include "shape_bullet.h"
+#include "soft_body_bullet.h"
+#include "space_bullet.h"
+
+class BulletPhysicsServer : public PhysicsServer {
+	GDCLASS(BulletPhysicsServer, PhysicsServer)
+
+	friend class BulletPhysicsDirectSpaceState;
+
+	bool active;
+	SpaceBullet *activeSpace;
+
+	mutable RID_Owner<SpaceBullet> space_owner;
+	mutable RID_Owner<ShapeBullet> shape_owner;
+	mutable RID_Owner<AreaBullet> area_owner;
+	mutable RID_Owner<RigidBodyBullet> rigid_body_owner;
+	mutable RID_Owner<SoftBodyBullet> soft_body_owner;
+	mutable RID_Owner<JointBullet> joint_owner;
+
+private:
+	/// This is used when a collision shape is not active, so the bullet compound shapes index are always sync with godot index
+	static btEmptyShape *emptyShape;
+
+public:
+	static btEmptyShape *get_empty_shape();
+
+protected:
+	static void _bind_methods();
+
+public:
+	BulletPhysicsServer();
+	~BulletPhysicsServer();
+
+	_FORCE_INLINE_ RID_Owner<SpaceBullet> *get_space_owner() {
+		return &space_owner;
+	}
+	_FORCE_INLINE_ RID_Owner<ShapeBullet> *get_shape_owner() {
+		return &shape_owner;
+	}
+	_FORCE_INLINE_ RID_Owner<AreaBullet> *get_area_owner() {
+		return &area_owner;
+	}
+	_FORCE_INLINE_ RID_Owner<RigidBodyBullet> *get_rigid_body_owner() {
+		return &rigid_body_owner;
+	}
+	_FORCE_INLINE_ RID_Owner<SoftBodyBullet> *get_soft_body_owner() {
+		return &soft_body_owner;
+	}
+	_FORCE_INLINE_ RID_Owner<JointBullet> *get_joint_owner() {
+		return &joint_owner;
+	}
+
+	/* SHAPE API */
+	virtual RID shape_create(ShapeType p_shape);
+	virtual void shape_set_data(RID p_shape, const Variant &p_data);
+	virtual ShapeType shape_get_type(RID p_shape) const;
+	virtual Variant shape_get_data(RID p_shape) const;
+
+	/// Not supported
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
+	/// Not supported
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
+
+	/* SPACE API */
+
+	virtual RID space_create();
+	virtual void space_set_active(RID p_space, bool p_active);
+	virtual bool space_is_active(RID p_space) const;
+
+	/// Not supported
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value);
+	/// Not supported
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const;
+
+	virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space);
+
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts);
+	virtual Vector<Vector3> space_get_contacts(RID p_space) const;
+	virtual int space_get_contact_count(RID p_space) const;
+
+	/* AREA API */
+
+	/// Bullet Physics Engine not support "Area", this must be handled by the game developer in another way.
+	/// Since godot Physics use the concept of area even to define the main world, the API area_set_param is used to set initial physics world information.
+	/// The API area_set_param is a bit hacky, and allow Godot to set some parameters on Bullet's world, a different use print a warning to console.
+	/// All other APIs returns a warning message if used
+
+	virtual RID area_create();
+
+	virtual void area_set_space(RID p_area, RID p_space);
+
+	virtual RID area_get_space(RID p_area) const;
+
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode);
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform());
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform);
+	virtual int area_get_shape_count(RID p_area) const;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const;
+	virtual void area_remove_shape(RID p_area, int p_shape_idx);
+	virtual void area_clear_shapes(RID p_area);
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled);
+	virtual void area_attach_object_instance_id(RID p_area, ObjectID p_ID);
+	virtual ObjectID area_get_object_instance_id(RID p_area) const;
+
+	/// If you pass as p_area the SpaceBullet you can set some parameters as specified below
+	/// AREA_PARAM_GRAVITY
+	/// AREA_PARAM_GRAVITY_VECTOR
+	/// Otherwise you can set area parameters
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value);
+	virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const;
+
+	virtual void area_set_transform(RID p_area, const Transform &p_transform);
+	virtual Transform area_get_transform(RID p_area) const;
+
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask);
+	virtual void area_set_collision_layer(RID p_area, uint32_t p_layer);
+
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable);
+	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
+	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
+	virtual void area_set_ray_pickable(RID p_area, bool p_enable);
+	virtual bool area_is_ray_pickable(RID p_area) const;
+
+	/* RIGID BODY API */
+
+	virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false);
+
+	virtual void body_set_space(RID p_body, RID p_space);
+	virtual RID body_get_space(RID p_body) const;
+
+	virtual void body_set_mode(RID p_body, BodyMode p_mode);
+	virtual BodyMode body_get_mode(RID p_body) const;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform());
+	// Not supported, Please remove and add new shape
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform);
+
+	virtual int body_get_shape_count(RID p_body) const;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
+
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx);
+	virtual void body_clear_shapes(RID p_body);
+
+	// Used for Rigid and Soft Bodies
+	virtual void body_attach_object_instance_id(RID p_body, uint32_t p_ID);
+	virtual uint32_t body_get_object_instance_id(RID p_body) const;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable);
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
+
+	virtual void body_set_collision_layer(RID p_body, uint32_t p_layer);
+	virtual uint32_t body_get_collision_layer(RID p_body) const;
+
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask);
+	virtual uint32_t body_get_collision_mask(RID p_body) const;
+
+	/// This is not supported by physics server
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
+	/// This is not supported by physics server
+	virtual uint32_t body_get_user_flags(RID p_body) const;
+
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
+
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void body_set_applied_force(RID p_body, const Vector3 &p_force);
+	virtual Vector3 body_get_applied_force(RID p_body) const;
+
+	virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque);
+	virtual Vector3 body_get_applied_torque(RID p_body) const;
+
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse);
+	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
+
+	virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock);
+	virtual BodyAxisLock body_get_axis_lock(RID p_body) const;
+
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
+	virtual int body_get_max_contacts_reported(RID p_body) const;
+
+	virtual void body_set_contacts_reported_depth_threshold(RID p_body, float p_treshold);
+	virtual float body_get_contacts_reported_depth_threshold(RID p_body) const;
+
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
+	virtual bool body_is_omitting_force_integration(RID p_body) const;
+
+	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
+
+	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
+	virtual bool body_is_ray_pickable(RID p_body) const;
+
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
+
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+
+	/* SOFT BODY API */
+
+	virtual RID soft_body_create(bool p_init_sleeping = false);
+
+	virtual void soft_body_set_space(RID p_body, RID p_space);
+	virtual RID soft_body_get_space(RID p_body) const;
+
+	virtual void soft_body_set_trimesh_body_shape(RID p_body, PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
+
+	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer);
+	virtual uint32_t soft_body_get_collision_layer(RID p_body) const;
+
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask);
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const;
+
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b);
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b);
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions);
+
+	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
+	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const;
+
+	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform);
+	virtual Transform soft_body_get_transform(RID p_body) const;
+
+	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable);
+	virtual bool soft_body_is_ray_pickable(RID p_body) const;
+
+	/* JOINT API */
+
+	virtual JointType joint_get_type(RID p_joint) const;
+
+	virtual void joint_set_solver_priority(RID p_joint, int p_priority);
+	virtual int joint_get_solver_priority(RID p_joint) const;
+
+	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
+
+	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value);
+	virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const;
+
+	virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_A);
+	virtual Vector3 pin_joint_get_local_a(RID p_joint) const;
+
+	virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B);
+	virtual Vector3 pin_joint_get_local_b(RID p_joint) const;
+
+	virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B);
+	virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B);
+
+	virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value);
+	virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const;
+
+	virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value);
+	virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const;
+
+	/// Reference frame is A
+	virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
+
+	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value);
+	virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const;
+
+	/// Reference frame is A
+	virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
+
+	virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value);
+	virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const;
+
+	/// Reference frame is A
+	virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B);
+
+	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, float p_value);
+	virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param);
+
+	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable);
+	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag);
+
+	/* MISC */
+
+	virtual void free(RID p_rid);
+
+	virtual void set_active(bool p_active) {
+		active = p_active;
+	}
+
+	static bool singleton_isActive() {
+		return static_cast<BulletPhysicsServer *>(get_singleton())->active;
+	}
+
+	bool isActive() {
+		return active;
+	}
+
+	virtual void init();
+	virtual void step(float p_deltaTime);
+	virtual void sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	virtual int get_process_info(ProcessInfo p_info);
+
+	CollisionObjectBullet *get_collisin_object(RID p_object) const;
+	RigidCollisionObjectBullet *get_rigid_collisin_object(RID p_object) const;
+
+	/// Internal APIs
+public:
+};
+
+#endif

modules/bullet/bullet_types_converter.cpp

@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  bullet_types_converter.cpp                                           */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#pragma once
+
+#include "bullet_types_converter.h"
+
+// ++ BULLET to GODOT ++++++++++
+void B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
+	outVal[0] = inVal[0];
+	outVal[1] = inVal[1];
+	outVal[2] = inVal[2];
+}
+
+void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal) {
+	outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
+	outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
+	outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
+}
+
+void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
+	B_TO_G(inVal[0], outVal[0]);
+	B_TO_G(inVal[1], outVal[1]);
+	B_TO_G(inVal[2], outVal[2]);
+}
+
+void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
+	INVERT_B_TO_G(inVal[0], outVal[0]);
+	INVERT_B_TO_G(inVal[1], outVal[1]);
+	INVERT_B_TO_G(inVal[2], outVal[2]);
+}
+
+void B_TO_G(btTransform const &inVal, Transform &outVal) {
+	B_TO_G(inVal.getBasis(), outVal.basis);
+	B_TO_G(inVal.getOrigin(), outVal.origin);
+}
+
+// ++ GODOT to BULLET ++++++++++
+void G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
+	outVal[0] = inVal[0];
+	outVal[1] = inVal[1];
+	outVal[2] = inVal[2];
+}
+
+void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal) {
+	outVal[0] = inVal[0] != 0. ? 1. / inVal[0] : 0.;
+	outVal[1] = inVal[1] != 0. ? 1. / inVal[1] : 0.;
+	outVal[2] = inVal[2] != 0. ? 1. / inVal[2] : 0.;
+}
+
+void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
+	G_TO_B(inVal[0], outVal[0]);
+	G_TO_B(inVal[1], outVal[1]);
+	G_TO_B(inVal[2], outVal[2]);
+}
+
+void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
+	INVERT_G_TO_B(inVal[0], outVal[0]);
+	INVERT_G_TO_B(inVal[1], outVal[1]);
+	INVERT_G_TO_B(inVal[2], outVal[2]);
+}
+
+void G_TO_B(Transform const &inVal, btTransform &outVal) {
+	G_TO_B(inVal.basis, outVal.getBasis());
+	G_TO_B(inVal.origin, outVal.getOrigin());
+}

modules/bullet/bullet_types_converter.h

@@ -0,0 +1,57 @@
+/*************************************************************************/
+/*  bullet_types_converter.h                                             */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BULLET_TYPES_CONVERTER_H
+#define BULLET_TYPES_CONVERTER_H
+
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "core/math/matrix3.h"
+#include "core/math/transform.h"
+#include "core/math/vector3.h"
+#include "core/typedefs.h"
+
+// Bullet to Godot
+extern void B_TO_G(btVector3 const &inVal, Vector3 &outVal);
+extern void INVERT_B_TO_G(btVector3 const &inVal, Vector3 &outVal);
+extern void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
+extern void INVERT_B_TO_G(btMatrix3x3 const &inVal, Basis &outVal);
+extern void B_TO_G(btTransform const &inVal, Transform &outVal);
+
+// Godot TO Bullet
+extern void G_TO_B(Vector3 const &inVal, btVector3 &outVal);
+extern void INVERT_G_TO_B(Vector3 const &inVal, btVector3 &outVal);
+extern void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
+extern void INVERT_G_TO_B(Basis const &inVal, btMatrix3x3 &outVal);
+extern void G_TO_B(Transform const &inVal, btTransform &outVal);
+
+#endif

modules/bullet/bullet_utilities.h

@@ -0,0 +1,44 @@
+/*************************************************************************/
+/*  bullet_utilities.h                                                   */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BULLET_UTILITIES_H
+#define BULLET_UTILITIES_H
+
+#pragma once
+
+#define bulletnew(cl) \
+	new cl
+
+#define bulletdelete(cl) \
+	delete cl;           \
+	cl = NULL;
+
+#endif

modules/bullet/collision_object_bullet.cpp

@@ -0,0 +1,316 @@
+/*************************************************************************/
+/*  collision_object_bullet.cpp                                          */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "collision_object_bullet.h"
+#include "area_bullet.h"
+#include "btBulletCollisionCommon.h"
+#include "bullet_physics_server.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "shape_bullet.h"
+#include "space_bullet.h"
+
+#define enableDynamicAabbTree true
+#define initialChildCapacity 1
+
+CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
+
+void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) {
+	G_TO_B(p_transform, transform);
+}
+void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
+	transform = p_transform;
+}
+
+CollisionObjectBullet::CollisionObjectBullet(Type p_type)
+	: RIDBullet(), space(NULL), type(p_type), collisionsEnabled(true), m_isStatic(false), bt_collision_object(NULL), body_scale(1., 1., 1.) {}
+
+CollisionObjectBullet::~CollisionObjectBullet() {
+	// Remove all overlapping
+	for (int i = areasOverlapped.size() - 1; 0 <= i; --i) {
+		areasOverlapped[i]->remove_overlapping_instantly(this);
+	}
+	// not required
+	// areasOverlapped.clear();
+
+	destroyBulletCollisionObject();
+}
+
+bool equal(real_t first, real_t second) {
+	return Math::abs(first - second) <= 0.001f;
+}
+
+void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
+	if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
+		G_TO_B(p_new_scale, body_scale);
+		on_body_scale_changed();
+	}
+}
+
+void CollisionObjectBullet::on_body_scale_changed() {
+}
+
+void CollisionObjectBullet::destroyBulletCollisionObject() {
+	bulletdelete(bt_collision_object);
+}
+
+void CollisionObjectBullet::setupBulletCollisionObject(btCollisionObject *p_collisionObject) {
+	bt_collision_object = p_collisionObject;
+	bt_collision_object->setUserPointer(this);
+	bt_collision_object->setUserIndex(type);
+	// Force the enabling of collision and avoid problems
+	set_collision_enabled(collisionsEnabled);
+}
+
+void CollisionObjectBullet::add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
+	exceptions.insert(p_ignoreCollisionObject->get_self());
+	bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, true);
+}
+
+void CollisionObjectBullet::remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
+	exceptions.erase(p_ignoreCollisionObject->get_self());
+	bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, false);
+}
+
+bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const {
+	return !bt_collision_object->checkCollideWithOverride(p_otherCollisionObject->bt_collision_object);
+}
+
+void CollisionObjectBullet::set_collision_enabled(bool p_enabled) {
+	collisionsEnabled = p_enabled;
+	if (collisionsEnabled) {
+		bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() & (~btCollisionObject::CF_NO_CONTACT_RESPONSE));
+	} else {
+		bt_collision_object->setCollisionFlags(bt_collision_object->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
+	}
+}
+
+bool CollisionObjectBullet::is_collisions_response_enabled() {
+	return collisionsEnabled;
+}
+
+void CollisionObjectBullet::notify_new_overlap(AreaBullet *p_area) {
+	areasOverlapped.push_back(p_area);
+}
+
+void CollisionObjectBullet::on_exit_area(AreaBullet *p_area) {
+	areasOverlapped.erase(p_area);
+}
+
+void CollisionObjectBullet::set_godot_object_flags(int flags) {
+	bt_collision_object->setUserIndex2(flags);
+}
+
+int CollisionObjectBullet::get_godot_object_flags() const {
+	return bt_collision_object->getUserIndex2();
+}
+
+void CollisionObjectBullet::set_transform(const Transform &p_global_transform) {
+
+	btTransform btTrans;
+	Basis decomposed_basis;
+
+	Vector3 decomposed_scale = p_global_transform.get_basis().rotref_posscale_decomposition(decomposed_basis);
+
+	G_TO_B(p_global_transform.get_origin(), btTrans.getOrigin());
+	G_TO_B(decomposed_basis, btTrans.getBasis());
+
+	set_body_scale(decomposed_scale);
+	set_transform__bullet(btTrans);
+}
+
+Transform CollisionObjectBullet::get_transform() const {
+	Transform t;
+	B_TO_G(get_transform__bullet(), t);
+	return t;
+}
+
+void CollisionObjectBullet::set_transform__bullet(const btTransform &p_global_transform) {
+	bt_collision_object->setWorldTransform(p_global_transform);
+}
+
+const btTransform &CollisionObjectBullet::get_transform__bullet() const {
+	return bt_collision_object->getWorldTransform();
+}
+
+RigidCollisionObjectBullet::RigidCollisionObjectBullet(Type p_type)
+	: CollisionObjectBullet(p_type), compoundShape(bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity))) {
+}
+
+RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
+	remove_all_shapes(true);
+	bt_collision_object->setCollisionShape(NULL);
+	bulletdelete(compoundShape);
+}
+
+/* Not used
+void RigidCollisionObjectBullet::_internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) {
+	bool at_least_one_was_changed = false;
+	btTransform old_transf;
+	// Inverse because I need remove the shapes
+	// Fetch all shapes to be sure to remove all shapes
+	for (int i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
+		if (compoundShape->getChildShape(i) == p_old_shape) {
+
+			old_transf = compoundShape->getChildTransform(i);
+			compoundShape->removeChildShapeByIndex(i);
+			compoundShape->addChildShape(old_transf, p_new_shape);
+			at_least_one_was_changed = true;
+		}
+	}
+
+	if (at_least_one_was_changed) {
+		on_shapes_changed();
+	}
+}*/
+
+void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform &p_transform) {
+	shapes.push_back(ShapeWrapper(p_shape, p_transform, true));
+	p_shape->add_owner(this);
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
+	ShapeWrapper &shp = shapes[p_index];
+	shp.shape->remove_owner(this);
+	p_shape->add_owner(this);
+	shp.shape = p_shape;
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
+	ERR_FAIL_INDEX(p_index, get_shape_count());
+
+	shapes[p_index].set_transform(p_transform);
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
+	// Remove the shape, all the times it appears
+	// Reverse order required for delete.
+	for (int i = shapes.size() - 1; 0 <= i; --i) {
+		if (p_shape == shapes[i].shape) {
+			internal_shape_destroy(i);
+			shapes.remove(i);
+		}
+	}
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::remove_shape(int p_index) {
+	ERR_FAIL_INDEX(p_index, get_shape_count());
+	internal_shape_destroy(p_index);
+	shapes.remove(p_index);
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody) {
+	// Reverse order required for delete.
+	for (int i = shapes.size() - 1; 0 <= i; --i) {
+		internal_shape_destroy(i, p_permanentlyFromThisBody);
+	}
+	shapes.clear();
+	on_shapes_changed();
+}
+
+int RigidCollisionObjectBullet::get_shape_count() const {
+	return shapes.size();
+}
+
+ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
+	return shapes[p_index].shape;
+}
+
+btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
+	return shapes[p_index].bt_shape;
+}
+
+Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
+	Transform trs;
+	B_TO_G(shapes[p_index].transform, trs);
+	return trs;
+}
+
+void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_shape) {
+	const int size = shapes.size();
+	for (int i = 0; i < size; ++i) {
+		if (shapes[i].shape == p_shape) {
+			bulletdelete(shapes[i].bt_shape);
+		}
+	}
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::on_shapes_changed() {
+	int i;
+	// Remove all shapes, reverse order for performance reason (Array resize)
+	for (i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
+		compoundShape->removeChildShapeByIndex(i);
+	}
+
+	// Insert all shapes
+	ShapeWrapper *shpWrapper;
+	const int size = shapes.size();
+	for (i = 0; i < size; ++i) {
+		shpWrapper = &shapes[i];
+		if (!shpWrapper->bt_shape) {
+			shpWrapper->bt_shape = shpWrapper->shape->create_bt_shape();
+		}
+		if (shpWrapper->active) {
+			compoundShape->addChildShape(shpWrapper->transform, shpWrapper->bt_shape);
+		} else {
+			compoundShape->addChildShape(shpWrapper->transform, BulletPhysicsServer::get_empty_shape());
+		}
+	}
+
+	compoundShape->setLocalScaling(body_scale);
+	compoundShape->recalculateLocalAabb();
+}
+
+void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
+	shapes[p_index].active = !p_disabled;
+	on_shapes_changed();
+}
+
+bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
+	return !shapes[p_index].active;
+}
+
+void RigidCollisionObjectBullet::on_body_scale_changed() {
+	CollisionObjectBullet::on_body_scale_changed();
+	on_shapes_changed();
+}
+
+void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {
+	ShapeWrapper &shp = shapes[p_index];
+	shp.shape->remove_owner(this, p_permanentlyFromThisBody);
+	bulletdelete(shp.bt_shape);
+}

modules/bullet/collision_object_bullet.h

@@ -0,0 +1,234 @@
+/*************************************************************************/
+/*  collision_object_bullet.h                                            */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef COLLISION_OBJECT_BULLET_H
+#define COLLISION_OBJECT_BULLET_H
+
+#include "LinearMath/btTransform.h"
+#include "core/vset.h"
+#include "object.h"
+#include "shape_owner_bullet.h"
+#include "transform.h"
+#include "vector3.h"
+
+class AreaBullet;
+class ShapeBullet;
+class btCollisionObject;
+class btCompoundShape;
+class btCollisionShape;
+class SpaceBullet;
+
+class CollisionObjectBullet : public RIDBullet {
+public:
+	enum GodotObjectFlags {
+		GOF_IS_MONITORING_AREA = 1 << 0
+		// FLAG2 = 1 << 1,
+		// FLAG3 = 1 << 2,
+		// FLAG4 = 1 << 3,
+		// FLAG5 = 1 << 4,
+		// FLAG6 = 1 << 5
+		// etc..
+	};
+	enum Type {
+		TYPE_AREA = 0,
+		TYPE_RIGID_BODY,
+		TYPE_SOFT_BODY,
+		TYPE_KINEMATIC_GHOST_BODY
+	};
+
+	struct ShapeWrapper {
+		ShapeBullet *shape;
+		btCollisionShape *bt_shape;
+		btTransform transform;
+		bool active;
+
+		ShapeWrapper()
+			: shape(NULL), bt_shape(NULL), active(true) {}
+
+		ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active)
+			: shape(p_shape), bt_shape(NULL), active(p_active) {
+			set_transform(p_transform);
+		}
+
+		ShapeWrapper(ShapeBullet *p_shape, const Transform &p_transform, bool p_active)
+			: shape(p_shape), bt_shape(NULL), active(p_active) {
+			set_transform(p_transform);
+		}
+		~ShapeWrapper();
+
+		ShapeWrapper(const ShapeWrapper &otherShape) {
+			operator=(otherShape);
+		}
+
+		void operator=(const ShapeWrapper &otherShape) {
+			shape = otherShape.shape;
+			bt_shape = otherShape.bt_shape;
+			transform = otherShape.transform;
+			active = otherShape.active;
+		}
+
+		void set_transform(const Transform &p_transform);
+		void set_transform(const btTransform &p_transform);
+	};
+
+protected:
+	Type type;
+	ObjectID instance_id;
+	uint32_t collisionLayer;
+	uint32_t collisionMask;
+	bool collisionsEnabled;
+	bool m_isStatic;
+	bool ray_pickable;
+	btCollisionObject *bt_collision_object;
+	btVector3 body_scale;
+	SpaceBullet *space;
+
+	VSet<RID> exceptions;
+
+	/// This array is used to know all areas where this Object is overlapped in
+	/// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea)
+	/// This array is used mainly to know which area hold the pointer of this object
+	Vector<AreaBullet *> areasOverlapped;
+
+public:
+	CollisionObjectBullet(Type p_type);
+	virtual ~CollisionObjectBullet();
+
+	Type getType() { return type; }
+
+protected:
+	void destroyBulletCollisionObject();
+	void setupBulletCollisionObject(btCollisionObject *p_collisionObject);
+
+public:
+	_FORCE_INLINE_ btCollisionObject *get_bt_collision_object() { return bt_collision_object; }
+
+	_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
+	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
+
+	_FORCE_INLINE_ bool is_static() const { return m_isStatic; }
+
+	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
+	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
+
+	void set_body_scale(const Vector3 &p_new_scale);
+	virtual void on_body_scale_changed();
+
+	void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
+	void remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
+	bool has_collision_exception(const CollisionObjectBullet *p_otherCollisionObject) const;
+	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
+
+	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
+		collisionLayer = p_layer;
+		on_collision_filters_change();
+	}
+	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collisionLayer; }
+
+	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
+		collisionMask = p_mask;
+		on_collision_filters_change();
+	}
+	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collisionMask; }
+
+	virtual void on_collision_filters_change() = 0;
+
+	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectBullet *p_other) const {
+		return collisionLayer & p_other->collisionMask || p_other->collisionLayer & collisionMask;
+	}
+
+	virtual void reload_body() = 0;
+	virtual void set_space(SpaceBullet *p_space) = 0;
+	_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
+	/// This is an event that is called when a collision checker starts
+	virtual void on_collision_checker_start() = 0;
+
+	virtual void dispatch_callbacks() = 0;
+
+	void set_collision_enabled(bool p_enabled);
+	bool is_collisions_response_enabled();
+
+	void notify_new_overlap(AreaBullet *p_area);
+	virtual void on_enter_area(AreaBullet *p_area) = 0;
+	virtual void on_exit_area(AreaBullet *p_area);
+
+	/// GodotObjectFlags
+	void set_godot_object_flags(int flags);
+	int get_godot_object_flags() const;
+
+	void set_transform(const Transform &p_global_transform);
+	Transform get_transform() const;
+	virtual void set_transform__bullet(const btTransform &p_global_transform);
+	virtual const btTransform &get_transform__bullet() const;
+};
+
+class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
+protected:
+	/// This is required to combine some shapes together.
+	/// Since Godot allow to have multiple shapes for each body with custom relative location,
+	/// each body will attach the shapes using this class even if there is only one shape.
+	btCompoundShape *compoundShape;
+	Vector<ShapeWrapper> shapes;
+
+public:
+	RigidCollisionObjectBullet(Type p_type);
+	~RigidCollisionObjectBullet();
+
+	_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
+
+	/// This is used to set new shape or replace existing
+	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
+	void add_shape(ShapeBullet *p_shape, const Transform &p_transform = Transform());
+	void set_shape(int p_index, ShapeBullet *p_shape);
+	void set_shape_transform(int p_index, const Transform &p_transform);
+	virtual void remove_shape(ShapeBullet *p_shape);
+	void remove_shape(int p_index);
+	void remove_all_shapes(bool p_permanentlyFromThisBody = false);
+
+	virtual void on_shape_changed(const ShapeBullet *const p_shape);
+	virtual void on_shapes_changed();
+
+	_FORCE_INLINE_ btCompoundShape *get_compound_shape() const { return compoundShape; }
+	int get_shape_count() const;
+	ShapeBullet *get_shape(int p_index) const;
+	btCollisionShape *get_bt_shape(int p_index) const;
+	Transform get_shape_transform(int p_index) const;
+
+	void set_shape_disabled(int p_index, bool p_disabled);
+	bool is_shape_disabled(int p_index);
+
+	virtual void on_body_scale_changed();
+
+private:
+	void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);
+};
+
+#endif

modules/bullet/cone_twist_joint_bullet.cpp

@@ -0,0 +1,111 @@
+/*************************************************************************/
+/*  cone_twist_joint_bullet.cpp                                          */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "cone_twist_joint_bullet.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "rigid_body_bullet.h"
+
+ConeTwistJointBullet::ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame)
+	: JointBullet() {
+	btTransform btFrameA;
+	G_TO_B(rbAFrame, btFrameA);
+	if (rbB) {
+		btTransform btFrameB;
+		G_TO_B(rbBFrame, btFrameB);
+		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
+	} else {
+		coneConstraint = bulletnew(btConeTwistConstraint(*rbA->get_bt_rigid_body(), btFrameA));
+	}
+	setup(coneConstraint);
+}
+
+void ConeTwistJointBullet::set_angular_only(bool angularOnly) {
+	coneConstraint->setAngularOnly(angularOnly);
+}
+
+void ConeTwistJointBullet::set_limit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness, real_t _biasFactor, real_t _relaxationFactor) {
+	coneConstraint->setLimit(_swingSpan1, _swingSpan2, _twistSpan, _softness, _biasFactor, _relaxationFactor);
+}
+
+int ConeTwistJointBullet::get_solve_twist_limit() {
+	return coneConstraint->getSolveTwistLimit();
+}
+
+int ConeTwistJointBullet::get_solve_swing_limit() {
+	return coneConstraint->getSolveSwingLimit();
+}
+
+real_t ConeTwistJointBullet::get_twist_limit_sign() {
+	return coneConstraint->getTwistLimitSign();
+}
+
+void ConeTwistJointBullet::set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
+			coneConstraint->setLimit(5, p_value);
+			coneConstraint->setLimit(4, p_value);
+			break;
+		case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
+			coneConstraint->setLimit(3, p_value);
+			break;
+		case PhysicsServer::CONE_TWIST_JOINT_BIAS:
+			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), p_value, coneConstraint->getRelaxationFactor());
+			break;
+		case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
+			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), p_value, coneConstraint->getBiasFactor(), coneConstraint->getRelaxationFactor());
+			break;
+		case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
+			coneConstraint->setLimit(coneConstraint->getSwingSpan1(), coneConstraint->getSwingSpan2(), coneConstraint->getTwistSpan(), coneConstraint->getLimitSoftness(), coneConstraint->getBiasFactor(), p_value);
+			break;
+		default:
+			WARN_PRINT("This parameter is not supported by Bullet engine");
+	}
+}
+
+real_t ConeTwistJointBullet::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN:
+			return coneConstraint->getSwingSpan1();
+		case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN:
+			return coneConstraint->getTwistSpan();
+		case PhysicsServer::CONE_TWIST_JOINT_BIAS:
+			return coneConstraint->getBiasFactor();
+		case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS:
+			return coneConstraint->getLimitSoftness();
+		case PhysicsServer::CONE_TWIST_JOINT_RELAXATION:
+			return coneConstraint->getRelaxationFactor();
+		default:
+			WARN_PRINT("This parameter is not supported by Bullet engine");
+			return 0;
+	}
+}

modules/bullet/cone_twist_joint_bullet.h

@@ -0,0 +1,58 @@
+/*************************************************************************/
+/*  cone_twist_joint_bullet.h                                            */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef CONE_TWIST_JOINT_BULLET_H
+#define CONE_TWIST_JOINT_BULLET_H
+
+#include "joint_bullet.h"
+
+class RigidBodyBullet;
+
+class ConeTwistJointBullet : public JointBullet {
+	class btConeTwistConstraint *coneConstraint;
+
+public:
+	ConeTwistJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
+
+	void set_angular_only(bool angularOnly);
+
+	void set_limit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f);
+	int get_solve_twist_limit();
+
+	int get_solve_swing_limit();
+	real_t get_twist_limit_sign();
+
+	void set_param(PhysicsServer::ConeTwistJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::ConeTwistJointParam p_param) const;
+};
+#endif

modules/bullet/config.py

@@ -0,0 +1,6 @@
+def can_build(platform):
+    return True
+
+def configure(env):
+    pass
+    

modules/bullet/constraint_bullet.cpp

@@ -0,0 +1,50 @@
+/*************************************************************************/
+/*  constraint_bullet.cpp                                                */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "constraint_bullet.h"
+#include "collision_object_bullet.h"
+#include "space_bullet.h"
+
+ConstraintBullet::ConstraintBullet()
+	: space(NULL), constraint(NULL) {}
+
+void ConstraintBullet::setup(btTypedConstraint *p_constraint) {
+	constraint = p_constraint;
+	constraint->setUserConstraintPtr(this);
+}
+
+void ConstraintBullet::set_space(SpaceBullet *p_space) {
+	space = p_space;
+}
+
+void ConstraintBullet::destroy_internal_constraint() {
+	space->remove_constraint(this);
+}

modules/bullet/constraint_bullet.h

@@ -0,0 +1,64 @@
+/*************************************************************************/
+/*  constraint_bullet.h                                                  */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef CONSTRAINT_BULLET_H
+#define CONSTRAINT_BULLET_H
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "bullet_utilities.h"
+#include "rid_bullet.h"
+
+class RigidBodyBullet;
+class SpaceBullet;
+class btTypedConstraint;
+
+class ConstraintBullet : public RIDBullet {
+
+protected:
+	SpaceBullet *space;
+	btTypedConstraint *constraint;
+
+public:
+	ConstraintBullet();
+
+	virtual void setup(btTypedConstraint *p_constraint);
+	virtual void set_space(SpaceBullet *p_space);
+	virtual void destroy_internal_constraint();
+
+public:
+	virtual ~ConstraintBullet() {
+		bulletdelete(constraint);
+		constraint = NULL;
+	}
+
+	_FORCE_INLINE_ btTypedConstraint *get_bt_constraint() { return constraint; }
+};
+#endif

modules/bullet/generic_6dof_joint_bullet.cpp

@@ -0,0 +1,241 @@
+/*************************************************************************/
+/*  generic_6dof_joint_bullet.cpp                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "generic_6dof_joint_bullet.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "rigid_body_bullet.h"
+
+Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
+	: JointBullet() {
+
+	btTransform btFrameA;
+	G_TO_B(frameInA, btFrameA);
+
+	if (rbB) {
+		btTransform btFrameB;
+		G_TO_B(frameInB, btFrameB);
+
+		sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, useLinearReferenceFrameA));
+	} else {
+		sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), btFrameA, useLinearReferenceFrameA));
+	}
+
+	setup(sixDOFConstraint);
+}
+
+Transform Generic6DOFJointBullet::getFrameOffsetA() const {
+	btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
+	Transform gTrs;
+	B_TO_G(btTrs, gTrs);
+	return gTrs;
+}
+
+Transform Generic6DOFJointBullet::getFrameOffsetB() const {
+	btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
+	Transform gTrs;
+	B_TO_G(btTrs, gTrs);
+	return gTrs;
+}
+
+Transform Generic6DOFJointBullet::getFrameOffsetA() {
+	btTransform btTrs = sixDOFConstraint->getFrameOffsetA();
+	Transform gTrs;
+	B_TO_G(btTrs, gTrs);
+	return gTrs;
+}
+
+Transform Generic6DOFJointBullet::getFrameOffsetB() {
+	btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
+	Transform gTrs;
+	B_TO_G(btTrs, gTrs);
+	return gTrs;
+}
+
+void Generic6DOFJointBullet::set_linear_lower_limit(const Vector3 &linearLower) {
+	btVector3 btVec;
+	G_TO_B(linearLower, btVec);
+	sixDOFConstraint->setLinearLowerLimit(btVec);
+}
+
+void Generic6DOFJointBullet::set_linear_upper_limit(const Vector3 &linearUpper) {
+	btVector3 btVec;
+	G_TO_B(linearUpper, btVec);
+	sixDOFConstraint->setLinearUpperLimit(btVec);
+}
+
+void Generic6DOFJointBullet::set_angular_lower_limit(const Vector3 &angularLower) {
+	btVector3 btVec;
+	G_TO_B(angularLower, btVec);
+	sixDOFConstraint->setAngularLowerLimit(btVec);
+}
+
+void Generic6DOFJointBullet::set_angular_upper_limit(const Vector3 &angularUpper) {
+	btVector3 btVec;
+	G_TO_B(angularUpper, btVec);
+	sixDOFConstraint->setAngularUpperLimit(btVec);
+}
+
+void Generic6DOFJointBullet::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+	switch (p_param) {
+		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
+			sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis] = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
+			sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis] = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
+			sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION:
+			sixDOFConstraint->getTranslationalLimitMotor()->m_restitution = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING:
+			sixDOFConstraint->getTranslationalLimitMotor()->m_damping = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_damping = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity = p_value;
+			break;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce = p_value;
+			break;
+		default:
+			WARN_PRINT("This parameter is not supported");
+	}
+}
+
+real_t Generic6DOFJointBullet::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, 0.);
+	switch (p_param) {
+		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT:
+			return sixDOFConstraint->getTranslationalLimitMotor()->m_lowerLimit[p_axis];
+		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT:
+			return sixDOFConstraint->getTranslationalLimitMotor()->m_upperLimit[p_axis];
+		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS:
+			return sixDOFConstraint->getTranslationalLimitMotor()->m_limitSoftness;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION:
+			return sixDOFConstraint->getTranslationalLimitMotor()->m_restitution;
+		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING:
+			return sixDOFConstraint->getTranslationalLimitMotor()->m_damping;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_loLimit;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_hiLimit;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_limitSoftness;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_damping;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_bounce;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_stopERP;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_targetVelocity;
+		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_maxLimitForce;
+		default:
+			WARN_PRINT("This parameter is not supported");
+			return 0.;
+	}
+}
+
+void Generic6DOFJointBullet::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
+	ERR_FAIL_INDEX(p_axis, 3);
+	switch (p_flag) {
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
+			if (p_value) {
+				if (!get_flag(p_axis, p_flag)) // avoid overwrite, if limited
+					sixDOFConstraint->setLimit(p_axis, 0, 0); // Limited
+			} else {
+				if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
+					sixDOFConstraint->setLimit(p_axis, 0, -1); // Free
+			}
+			break;
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			int angularAxis = 3 + p_axis;
+			if (p_value) {
+				if (!get_flag(p_axis, p_flag)) // avoid overwrite, if Limited
+					sixDOFConstraint->setLimit(angularAxis, 0, 0); // Limited
+			} else {
+				if (get_flag(p_axis, p_flag)) // avoid overwrite, if free
+					sixDOFConstraint->setLimit(angularAxis, 0, -1); // Free
+			}
+			break;
+		}
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
+			//sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] = p_value;
+			sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor = p_value;
+			break;
+		default:
+			WARN_PRINT("This flag is not supported by Bullet engine");
+	}
+}
+
+bool Generic6DOFJointBullet::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, false);
+	switch (p_flag) {
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT:
+			return sixDOFConstraint->getTranslationalLimitMotor()->isLimited(p_axis);
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT:
+			return sixDOFConstraint->getRotationalLimitMotor(p_axis)->isLimited();
+		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR:
+			return //sixDOFConstraint->getTranslationalLimitMotor()->m_enableMotor[p_axis] &&
+					sixDOFConstraint->getRotationalLimitMotor(p_axis)->m_enableMotor;
+		default:
+			WARN_PRINT("This flag is not supported by Bullet engine");
+			return false;
+	}
+}

modules/bullet/generic_6dof_joint_bullet.h

@@ -0,0 +1,65 @@
+/*************************************************************************/
+/*  generic_6dof_joint_bullet.h                                          */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GENERIC_6DOF_JOINT_BULLET_H
+#define GENERIC_6DOF_JOINT_BULLET_H
+
+#include "joint_bullet.h"
+
+class RigidBodyBullet;
+
+class Generic6DOFJointBullet : public JointBullet {
+	class btGeneric6DofConstraint *sixDOFConstraint;
+
+public:
+	Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
+
+	Transform getFrameOffsetA() const;
+	Transform getFrameOffsetB() const;
+	Transform getFrameOffsetA();
+	Transform getFrameOffsetB();
+
+	void set_linear_lower_limit(const Vector3 &linearLower);
+	void set_linear_upper_limit(const Vector3 &linearUpper);
+
+	void set_angular_lower_limit(const Vector3 &angularLower);
+	void set_angular_upper_limit(const Vector3 &angularUpper);
+
+	void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, real_t p_value);
+	real_t get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const;
+
+	void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
+	bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const;
+};
+
+#endif

modules/bullet/godot_collision_configuration.cpp

@@ -0,0 +1,91 @@
+/*************************************************************************/
+/*  godot_collision_configuration.cpp                                    */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "godot_collision_configuration.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "godot_ray_world_algorithm.h"
+
+GodotCollisionConfiguration::GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo)
+	: btDefaultCollisionConfiguration(constructionInfo) {
+
+	void *mem = NULL;
+
+	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::CreateFunc), 16);
+	m_rayWorldCF = new (mem) GodotRayWorldAlgorithm::CreateFunc(world);
+
+	mem = btAlignedAlloc(sizeof(GodotRayWorldAlgorithm::SwappedCreateFunc), 16);
+	m_swappedRayWorldCF = new (mem) GodotRayWorldAlgorithm::SwappedCreateFunc(world);
+}
+
+GodotCollisionConfiguration::~GodotCollisionConfiguration() {
+	m_rayWorldCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(m_rayWorldCF);
+
+	m_swappedRayWorldCF->~btCollisionAlgorithmCreateFunc();
+	btAlignedFree(m_swappedRayWorldCF);
+}
+
+btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) {
+
+	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
+
+		// This collision is not supported
+		return m_emptyCreateFunc;
+	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
+
+		return m_rayWorldCF;
+	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
+
+		return m_swappedRayWorldCF;
+	} else {
+
+		return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0, proxyType1);
+	}
+}
+
+btCollisionAlgorithmCreateFunc *GodotCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) {
+
+	if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0 && CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
+
+		// This collision is not supported
+		return m_emptyCreateFunc;
+	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType0) {
+
+		return m_rayWorldCF;
+	} else if (CUSTOM_CONVEX_SHAPE_TYPE == proxyType1) {
+
+		return m_swappedRayWorldCF;
+	} else {
+
+		return btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(proxyType0, proxyType1);
+	}
+}

modules/bullet/godot_collision_configuration.h

@@ -0,0 +1,50 @@
+/*************************************************************************/
+/*  godot_collision_configuration.h                                      */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GODOT_COLLISION_CONFIGURATION_H
+#define GODOT_COLLISION_CONFIGURATION_H
+
+#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
+
+class btDiscreteDynamicsWorld;
+
+class GodotCollisionConfiguration : public btDefaultCollisionConfiguration {
+	btCollisionAlgorithmCreateFunc *m_rayWorldCF;
+	btCollisionAlgorithmCreateFunc *m_swappedRayWorldCF;
+
+public:
+	GodotCollisionConfiguration(const btDiscreteDynamicsWorld *world, const btDefaultCollisionConstructionInfo &constructionInfo = btDefaultCollisionConstructionInfo());
+	virtual ~GodotCollisionConfiguration();
+
+	virtual btCollisionAlgorithmCreateFunc *getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1);
+	virtual btCollisionAlgorithmCreateFunc *getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
+};
+#endif

modules/bullet/godot_collision_dispatcher.cpp

@@ -0,0 +1,54 @@
+/*************************************************************************/
+/*  godot_collision_dispatcher.cpp                                       */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "godot_collision_dispatcher.h"
+#include "collision_object_bullet.h"
+
+const int GodotCollisionDispatcher::CASTED_TYPE_AREA = static_cast<int>(CollisionObjectBullet::TYPE_AREA);
+
+GodotCollisionDispatcher::GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration)
+	: btCollisionDispatcher(collisionConfiguration) {}
+
+bool GodotCollisionDispatcher::needsCollision(const btCollisionObject *body0, const btCollisionObject *body1) {
+	if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
+		// Avoide area narrow phase
+		return false;
+	}
+	return btCollisionDispatcher::needsCollision(body0, body1);
+}
+
+bool GodotCollisionDispatcher::needsResponse(const btCollisionObject *body0, const btCollisionObject *body1) {
+	if (body0->getUserIndex() == CASTED_TYPE_AREA || body1->getUserIndex() == CASTED_TYPE_AREA) {
+		// Avoide area narrow phase
+		return false;
+	}
+	return btCollisionDispatcher::needsResponse(body0, body1);
+}

modules/bullet/godot_collision_dispatcher.h

@@ -0,0 +1,48 @@
+/*************************************************************************/
+/*  godot_collision_dispatcher.h                                         */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GODOT_COLLISION_DISPATCHER_H
+#define GODOT_COLLISION_DISPATCHER_H
+
+#include "int_types.h"
+#include <btBulletDynamicsCommon.h>
+
+/// This class is required to implement custom collision behaviour in the narrowphase
+class GodotCollisionDispatcher : public btCollisionDispatcher {
+private:
+	static const int CASTED_TYPE_AREA;
+
+public:
+	GodotCollisionDispatcher(btCollisionConfiguration *collisionConfiguration);
+	virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1);
+	virtual bool needsResponse(const btCollisionObject *body0, const btCollisionObject *body1);
+};
+#endif

modules/bullet/godot_motion_state.h

@@ -0,0 +1,96 @@
+/*************************************************************************/
+/*  godot_motion_state.h                                                 */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GODOT_MOTION_STATE_H
+#define GODOT_MOTION_STATE_H
+
+#include "LinearMath/btMotionState.h"
+#include "rigid_body_bullet.h"
+
+class RigidBodyBullet;
+
+// This clas is responsible to move kinematic actor
+// and sincronize rendering engine with Bullet
+/// DOC:
+/// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/MotionStates#What.27s_a_MotionState.3F
+class GodotMotionState : public btMotionState {
+
+	/// This data is used to store the new world position for kinematic body
+	btTransform bodyKinematicWorldTransf;
+	/// This data is used to store last world position
+	btTransform bodyCurrentWorldTransform;
+
+	RigidBodyBullet *owner;
+
+public:
+	GodotMotionState(RigidBodyBullet *p_owner)
+		: bodyKinematicWorldTransf(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
+		  bodyCurrentWorldTransform(btMatrix3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.), btVector3(0., 0., 0.)),
+		  owner(p_owner) {}
+
+	/// IMPORTANT DON'T USE THIS FUNCTION TO KNOW THE CURRENT BODY TRANSFORM
+	/// This class is used internally by Bullet
+	/// Use GodotMotionState::getCurrentWorldTransform to know current position
+	///
+	/// This function is used by Bullet to get the position of object in the world
+	/// if the body is kinematic Bullet will move the object to this location
+	/// if the body is static Bullet doesn't move at all
+	virtual void getWorldTransform(btTransform &worldTrans) const {
+		worldTrans = bodyKinematicWorldTransf;
+	}
+
+	/// IMPORTANT: to move the body use: moveBody
+	/// IMPORTANT: DON'T CALL THIS FUNCTION, IT IS CALLED BY BULLET TO UPDATE RENDERING ENGINE
+	///
+	/// This function is called each time by Bullet and set the current position of body
+	/// inside the physics world.
+	/// Don't allow Godot rendering scene takes world transform from this object because
+	/// the correct transform is set by Bullet only after the last step when there are sub steps
+	/// This function must update Godot transform rendering scene for this object.
+	virtual void setWorldTransform(const btTransform &worldTrans) {
+		bodyCurrentWorldTransform = worldTrans;
+
+		owner->scratch();
+	}
+
+public:
+	/// Use this function to move kinematic body
+	/// -- or set initial transfom before body creation.
+	void moveBody(const btTransform &newWorldTransform) {
+		bodyKinematicWorldTransf = newWorldTransform;
+	}
+
+	/// It returns the current body transform from last Bullet update
+	const btTransform &getCurrentWorldTransform() const {
+		return bodyCurrentWorldTransform;
+	}
+};
+#endif

modules/bullet/godot_ray_world_algorithm.cpp

@@ -0,0 +1,104 @@
+/*************************************************************************/
+/*  godot_ray_world_algorithm.cpp                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "godot_ray_world_algorithm.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "btRayShape.h"
+#include "collision_object_bullet.h"
+
+GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world)
+	: m_world(world) {}
+
+GodotRayWorldAlgorithm::SwappedCreateFunc::SwappedCreateFunc(const btDiscreteDynamicsWorld *world)
+	: m_world(world) {}
+
+GodotRayWorldAlgorithm::GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_manifoldPtr(mf),
+	  m_ownManifold(false),
+	  m_world(world),
+	  m_isSwapped(isSwapped) {}
+
+GodotRayWorldAlgorithm::~GodotRayWorldAlgorithm() {
+	if (m_ownManifold && m_manifoldPtr) {
+		m_dispatcher->releaseManifold(m_manifoldPtr);
+	}
+}
+
+void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
+
+	if (!m_manifoldPtr) {
+		if (m_isSwapped) {
+			m_manifoldPtr = m_dispatcher->getNewManifold(body1Wrap->getCollisionObject(), body0Wrap->getCollisionObject());
+		} else {
+			m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
+		}
+		m_ownManifold = true;
+	}
+	m_manifoldPtr->clearManifold();
+	resultOut->setPersistentManifold(m_manifoldPtr);
+
+	const btRayShape *ray_shape;
+	btTransform ray_transform;
+
+	const btCollisionObjectWrapper *other_co_wrapper;
+
+	if (m_isSwapped) {
+
+		ray_shape = static_cast<const btRayShape *>(body1Wrap->getCollisionShape());
+		ray_transform = body1Wrap->getWorldTransform();
+
+		other_co_wrapper = body0Wrap;
+	} else {
+
+		ray_shape = static_cast<const btRayShape *>(body0Wrap->getCollisionShape());
+		ray_transform = body0Wrap->getWorldTransform();
+
+		other_co_wrapper = body1Wrap;
+	}
+
+	btTransform to(ray_transform * ray_shape->getSupportPoint());
+
+	btCollisionWorld::ClosestRayResultCallback btResult(ray_transform.getOrigin(), to.getOrigin());
+
+	m_world->rayTestSingleInternal(ray_transform, to, other_co_wrapper, btResult);
+
+	if (btResult.hasHit()) {
+		btVector3 ray_normal(to.getOrigin() - ray_transform.getOrigin());
+		ray_normal.normalize();
+		ray_normal *= -1;
+		resultOut->addContactPoint(ray_normal, btResult.m_hitPointWorld, ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
+	}
+}
+
+btScalar GodotRayWorldAlgorithm::calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut) {
+	return 1;
+}

modules/bullet/godot_ray_world_algorithm.h

@@ -0,0 +1,83 @@
+/*************************************************************************/
+/*  godot_ray_world_algorithm.h                                          */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GODOT_RAY_WORLD_ALGORITHM_H
+#define GODOT_RAY_WORLD_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+
+class btDiscreteDynamicsWorld;
+
+class GodotRayWorldAlgorithm : public btActivatingCollisionAlgorithm {
+
+	const btDiscreteDynamicsWorld *m_world;
+	btPersistentManifold *m_manifoldPtr;
+	bool m_ownManifold;
+	bool m_isSwapped;
+
+public:
+	GodotRayWorldAlgorithm(const btDiscreteDynamicsWorld *m_world, btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped);
+	virtual ~GodotRayWorldAlgorithm();
+
+	virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
+	virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut);
+
+	virtual void getAllContactManifolds(btManifoldArray &manifoldArray) {
+		///should we use m_ownManifold to avoid adding duplicates?
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+	struct CreateFunc : public btCollisionAlgorithmCreateFunc {
+
+		const btDiscreteDynamicsWorld *m_world;
+		CreateFunc(const btDiscreteDynamicsWorld *world);
+
+		virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
+			void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
+			return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, false);
+		}
+	};
+
+	struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc {
+
+		const btDiscreteDynamicsWorld *m_world;
+		SwappedCreateFunc(const btDiscreteDynamicsWorld *world);
+
+		virtual btCollisionAlgorithm *CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) {
+			void *mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(GodotRayWorldAlgorithm));
+			return new (mem) GodotRayWorldAlgorithm(m_world, ci.m_manifold, ci, body0Wrap, body1Wrap, true);
+		}
+	};
+};
+
+#endif // GODOT_RAY_WORLD_ALGORITHM_H

modules/bullet/godot_result_callbacks.cpp

@@ -0,0 +1,291 @@
+/*************************************************************************/
+/*  godot_result_callbacks.cpp                                           */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "godot_result_callbacks.h"
+#include "bullet_types_converter.h"
+#include "collision_object_bullet.h"
+#include "rigid_body_bullet.h"
+
+bool GodotFilterCallback::test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask) {
+	return body0_collision_layer & body1_collision_mask || body1_collision_layer & body0_collision_mask;
+}
+
+bool GodotFilterCallback::needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const {
+	return GodotFilterCallback::test_collision_filters(proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask, proxy1->m_collisionFilterGroup, proxy1->m_collisionFilterMask);
+}
+
+bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_pickRay && gObj->is_ray_pickable()) {
+			return true;
+		} else if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
+	CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(convexResult.m_hitCollisionObject->getUserPointer());
+
+	PhysicsDirectSpaceState::ShapeResult &result = m_results[count];
+
+	result.shape = convexResult.m_localShapeInfo->m_shapePart;
+	result.rid = gObj->get_self();
+	result.collider_id = gObj->get_instance_id();
+	result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
+
+	++count;
+	return count < m_resultMax;
+}
+
+bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (gObj == m_self_object) {
+			return false;
+		} else {
+			if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
+				return false;
+			} else if (m_self_object->has_collision_exception(gObj)) {
+				return false;
+			}
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
+	btScalar res = btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
+	m_shapePart = convexResult.m_localShapeInfo->m_shapePart;
+	return res;
+}
+
+bool GodotAllContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotAllContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
+
+	if (cp.getDistance() <= 0) {
+
+		PhysicsDirectSpaceState::ShapeResult &result = m_results[m_count];
+		// Penetrated
+
+		CollisionObjectBullet *colObj;
+		if (m_self_object == colObj0Wrap->getCollisionObject()) {
+			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
+			result.shape = cp.m_index1;
+		} else {
+			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
+			result.shape = cp.m_index0;
+		}
+
+		if (colObj)
+			result.collider_id = colObj->get_instance_id();
+		else
+			result.collider_id = 0;
+		result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
+		result.rid = colObj->get_self();
+		++m_count;
+	}
+
+	return m_count < m_resultMax;
+}
+
+bool GodotContactPairContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
+
+	if (m_self_object == colObj0Wrap->getCollisionObject()) {
+		B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 0]); // Local contact
+		B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 1]);
+	} else {
+		B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 0]); // Local contact
+		B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 1]);
+	}
+
+	++m_count;
+
+	return m_count < m_resultMax;
+}
+
+bool GodotRestInfoContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (m_exclude->has(gObj->get_self())) {
+			return false;
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
+
+	if (cp.getDistance() <= m_min_distance) {
+		m_min_distance = cp.getDistance();
+
+		CollisionObjectBullet *colObj;
+		if (m_self_object == colObj0Wrap->getCollisionObject()) {
+			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
+			m_result->shape = cp.m_index1;
+			B_TO_G(cp.getPositionWorldOnB(), m_result->point);
+			m_rest_info_bt_point = cp.getPositionWorldOnB();
+			m_rest_info_collision_object = colObj1Wrap->getCollisionObject();
+		} else {
+			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
+			m_result->shape = cp.m_index0;
+			B_TO_G(cp.m_normalWorldOnB * -1, m_result->normal);
+			m_rest_info_bt_point = cp.getPositionWorldOnA();
+			m_rest_info_collision_object = colObj0Wrap->getCollisionObject();
+		}
+
+		if (colObj)
+			m_result->collider_id = colObj->get_instance_id();
+		else
+			m_result->collider_id = 0;
+		m_result->rid = colObj->get_self();
+
+		m_collided = true;
+	}
+
+	return cp.getDistance();
+}
+
+bool GodotRecoverAndClosestContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
+	if (needs) {
+		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		if (gObj == m_self_object) {
+			return false;
+		} else {
+			if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
+				return false;
+			} else if (m_self_object->has_collision_exception(gObj)) {
+				return false;
+			}
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+btScalar GodotRecoverAndClosestContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
+
+	if (cp.getDistance() < -MAX_PENETRATION_DEPTH) {
+		if (m_most_penetrated_distance > cp.getDistance()) {
+			m_most_penetrated_distance = cp.getDistance();
+
+			// take other object
+			btScalar sign(1);
+			if (m_self_object == colObj0Wrap->getCollisionObject()->getUserPointer()) {
+				m_pointCollisionObject = colObj1Wrap->getCollisionObject();
+				m_other_compound_shape_index = cp.m_index1;
+			} else {
+				m_pointCollisionObject = colObj0Wrap->getCollisionObject();
+				sign = -1;
+				m_other_compound_shape_index = cp.m_index0;
+			}
+
+			m_pointNormalWorld = cp.m_normalWorldOnB * sign;
+			m_pointWorld = cp.getPositionWorldOnB();
+			m_penetration_distance = cp.getDistance();
+
+			m_recover_penetration -= cp.m_normalWorldOnB * sign * (cp.getDistance() + MAX_PENETRATION_DEPTH);
+		}
+	}
+	return 1;
+}

modules/bullet/godot_result_callbacks.h

@@ -0,0 +1,189 @@
+/*************************************************************************/
+/*  godot_result_callbacks.h                                             */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef GODOT_RESULT_CALLBACKS_H
+#define GODOT_RESULT_CALLBACKS_H
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "btBulletDynamicsCommon.h"
+#include "servers/physics_server.h"
+
+#define MAX_PENETRATION_DEPTH 0.005
+
+class RigidBodyBullet;
+
+/// This class is required to implement custom collision behaviour in the broadphase
+struct GodotFilterCallback : public btOverlapFilterCallback {
+	static bool test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask);
+
+	// return true when pairs need collision
+	virtual bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const;
+};
+
+/// It performs an additional check allow exclusions.
+struct GodotClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback {
+	const Set<RID> *m_exclude;
+	bool m_pickRay;
+
+public:
+	GodotClosestRayResultCallback(const btVector3 &rayFromWorld, const btVector3 &rayToWorld, const Set<RID> *p_exclude)
+		: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), m_exclude(p_exclude), m_pickRay(false) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+};
+
+// store all colliding object
+struct GodotAllConvexResultCallback : public btCollisionWorld::ConvexResultCallback {
+public:
+	PhysicsDirectSpaceState::ShapeResult *m_results;
+	int m_resultMax;
+	int count;
+	const Set<RID> *m_exclude;
+
+	GodotAllConvexResultCallback(PhysicsDirectSpaceState::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude)
+		: m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), count(0) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
+};
+
+struct GodotKinClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
+public:
+	const RigidBodyBullet *m_self_object;
+	const bool m_ignore_areas;
+
+	GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_ignore_areas)
+		: btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+};
+
+struct GodotClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
+public:
+	const Set<RID> *m_exclude;
+	int m_shapePart;
+
+	GodotClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const Set<RID> *p_exclude)
+		: btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld), m_exclude(p_exclude) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace);
+};
+
+struct GodotAllContactResultCallback : public btCollisionWorld::ContactResultCallback {
+public:
+	const btCollisionObject *m_self_object;
+	PhysicsDirectSpaceState::ShapeResult *m_results;
+	int m_resultMax;
+	int m_count;
+	const Set<RID> *m_exclude;
+
+	GodotAllContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::ShapeResult *p_results, int p_resultMax, const Set<RID> *p_exclude)
+		: m_self_object(p_self_object), m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), m_count(0) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
+};
+
+/// Returns the list of contacts pairs in this order: Local contact, other body contact
+struct GodotContactPairContactResultCallback : public btCollisionWorld::ContactResultCallback {
+public:
+	const btCollisionObject *m_self_object;
+	Vector3 *m_results;
+	int m_resultMax;
+	int m_count;
+	const Set<RID> *m_exclude;
+
+	GodotContactPairContactResultCallback(btCollisionObject *p_self_object, Vector3 *p_results, int p_resultMax, const Set<RID> *p_exclude)
+		: m_self_object(p_self_object), m_results(p_results), m_exclude(p_exclude), m_resultMax(p_resultMax), m_count(0) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
+};
+
+struct GodotRestInfoContactResultCallback : public btCollisionWorld::ContactResultCallback {
+public:
+	const btCollisionObject *m_self_object;
+	PhysicsDirectSpaceState::ShapeRestInfo *m_result;
+	bool m_collided;
+	real_t m_min_distance;
+	const btCollisionObject *m_rest_info_collision_object;
+	btVector3 m_rest_info_bt_point;
+	const Set<RID> *m_exclude;
+
+	GodotRestInfoContactResultCallback(btCollisionObject *p_self_object, PhysicsDirectSpaceState::ShapeRestInfo *p_result, const Set<RID> *p_exclude)
+		: m_self_object(p_self_object), m_result(p_result), m_exclude(p_exclude), m_collided(false), m_min_distance(0) {}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
+};
+
+struct GodotRecoverAndClosestContactResultCallback : public btCollisionWorld::ContactResultCallback {
+public:
+	btVector3 m_pointNormalWorld;
+	btVector3 m_pointWorld;
+	btScalar m_penetration_distance;
+	int m_other_compound_shape_index;
+	const btCollisionObject *m_pointCollisionObject;
+
+	const RigidBodyBullet *m_self_object;
+	bool m_ignore_areas;
+
+	btScalar m_most_penetrated_distance;
+	btVector3 m_recover_penetration;
+
+	GodotRecoverAndClosestContactResultCallback()
+		: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(NULL), m_ignore_areas(true), m_most_penetrated_distance(1e20), m_recover_penetration(0, 0, 0) {}
+
+	GodotRecoverAndClosestContactResultCallback(const RigidBodyBullet *p_self_object, bool p_ignore_areas)
+		: m_pointCollisionObject(NULL), m_penetration_distance(0), m_other_compound_shape_index(0), m_self_object(p_self_object), m_ignore_areas(p_ignore_areas), m_most_penetrated_distance(9999999999), m_recover_penetration(0, 0, 0) {}
+
+	void reset() {
+		m_pointCollisionObject = NULL;
+		m_most_penetrated_distance = 1e20;
+		m_recover_penetration.setZero();
+	}
+
+	bool hasHit() {
+		return m_pointCollisionObject;
+	}
+
+	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
+
+	virtual btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
+};
+
+#endif // GODOT_RESULT_CALLBACKS_H

modules/bullet/hinge_joint_bullet.cpp

@@ -0,0 +1,163 @@
+/*************************************************************************/
+/*  hinge_joint_bullet.cpp                                               */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "hinge_joint_bullet.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "rigid_body_bullet.h"
+
+HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB)
+	: JointBullet() {
+	btTransform btFrameA;
+	G_TO_B(frameA, btFrameA);
+
+	if (rbB) {
+		btTransform btFrameB;
+		G_TO_B(frameB, btFrameB);
+
+		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
+	} else {
+
+		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btFrameA));
+	}
+
+	setup(hingeConstraint);
+}
+
+HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB)
+	: JointBullet() {
+
+	btVector3 btPivotA;
+	btVector3 btAxisA;
+	G_TO_B(pivotInA, btPivotA);
+	G_TO_B(axisInA, btAxisA);
+
+	if (rbB) {
+		btVector3 btPivotB;
+		btVector3 btAxisB;
+		G_TO_B(pivotInB, btPivotB);
+		G_TO_B(axisInB, btAxisB);
+
+		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btPivotA, btPivotB, btAxisA, btAxisB));
+	} else {
+
+		hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btPivotA, btAxisA));
+	}
+
+	setup(hingeConstraint);
+}
+
+real_t HingeJointBullet::get_hinge_angle() {
+	return hingeConstraint->getHingeAngle();
+}
+
+void HingeJointBullet::set_param(PhysicsServer::HingeJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::HINGE_JOINT_BIAS:
+			if (0 < p_value) {
+				print_line("The Bullet Hinge Joint doesn't support bias, So it's always 0");
+			}
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
+			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), p_value, hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
+			hingeConstraint->setLimit(p_value, hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
+			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), p_value, hingeConstraint->getLimitRelaxationFactor());
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
+			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), p_value, hingeConstraint->getLimitBiasFactor(), hingeConstraint->getLimitRelaxationFactor());
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
+			hingeConstraint->setLimit(hingeConstraint->getLowerLimit(), hingeConstraint->getUpperLimit(), hingeConstraint->getLimitSoftness(), hingeConstraint->getLimitBiasFactor(), p_value);
+			break;
+		case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			hingeConstraint->setMotorTargetVelocity(p_value);
+			break;
+		case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			hingeConstraint->setMaxMotorImpulse(p_value);
+			break;
+		default:
+			WARN_PRINTS("The Bullet Hinge Joint doesn't support this parameter: " + itos(p_param) + ", value: " + itos(p_value));
+	}
+}
+
+real_t HingeJointBullet::get_param(PhysicsServer::HingeJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer::HINGE_JOINT_BIAS:
+			return 0;
+			break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_UPPER:
+			return hingeConstraint->getUpperLimit();
+		case PhysicsServer::HINGE_JOINT_LIMIT_LOWER:
+			return hingeConstraint->getLowerLimit();
+		case PhysicsServer::HINGE_JOINT_LIMIT_BIAS:
+			return hingeConstraint->getLimitBiasFactor();
+		case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS:
+			return hingeConstraint->getLimitSoftness();
+		case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION:
+			return hingeConstraint->getLimitRelaxationFactor();
+		case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY:
+			return hingeConstraint->getMotorTargetVelocity();
+		case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE:
+			return hingeConstraint->getMaxMotorImpulse();
+		default:
+			WARN_PRINTS("The Bullet Hinge Joint doesn't support this parameter: " + itos(p_param));
+			return 0;
+	}
+}
+
+void HingeJointBullet::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) {
+	switch (p_flag) {
+		case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
+			if (!p_value) {
+				hingeConstraint->setLimit(-Math_PI, Math_PI);
+			}
+			break;
+		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			hingeConstraint->enableMotor(p_value);
+			break;
+	}
+}
+
+bool HingeJointBullet::get_flag(PhysicsServer::HingeJointFlag p_flag) const {
+	switch (p_flag) {
+		case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT:
+			return true;
+		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:
+			return hingeConstraint->getEnableAngularMotor();
+		default:
+			return false;
+	}
+}

modules/bullet/hinge_joint_bullet.h

@@ -0,0 +1,54 @@
+/*************************************************************************/
+/*  hinge_joint_bullet.h                                                 */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef HINGE_JOINT_BULLET_H
+#define HINGE_JOINT_BULLET_H
+
+#include "joint_bullet.h"
+
+class HingeJointBullet : public JointBullet {
+	class btHingeConstraint *hingeConstraint;
+
+public:
+	HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB);
+	HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
+
+	real_t get_hinge_angle();
+
+	void set_param(PhysicsServer::HingeJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::HingeJointParam p_param) const;
+
+	void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
+	bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
+};
+#endif

modules/bullet/joint_bullet.cpp

@@ -0,0 +1,38 @@
+/*************************************************************************/
+/*  joint_bullet.cpp                                                     */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "joint_bullet.h"
+#include "space_bullet.h"
+
+JointBullet::JointBullet()
+	: ConstraintBullet() {}
+
+JointBullet::~JointBullet() {}

modules/bullet/joint_bullet.h

@@ -0,0 +1,49 @@
+/*************************************************************************/
+/*  joint_bullet.h                                                       */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef JOINT_BULLET_H
+#define JOINT_BULLET_H
+
+#include "constraint_bullet.h"
+#include "servers/physics_server.h"
+
+class RigidBodyBullet;
+class btTypedConstraint;
+
+class JointBullet : public ConstraintBullet {
+
+public:
+	JointBullet();
+	virtual ~JointBullet();
+
+	virtual PhysicsServer::JointType get_type() const = 0;
+};
+#endif

modules/bullet/pin_joint_bullet.cpp

@@ -0,0 +1,112 @@
+/*************************************************************************/
+/*  pin_joint_bullet.cpp                                                 */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "pin_joint_bullet.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "bullet_types_converter.h"
+#include "rigid_body_bullet.h"
+
+PinJointBullet::PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b)
+	: JointBullet() {
+	if (p_body_b) {
+
+		btVector3 btPivotA;
+		btVector3 btPivotB;
+		G_TO_B(p_pos_a, btPivotA);
+		G_TO_B(p_pos_b, btPivotB);
+		p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(),
+				*p_body_b->get_bt_rigid_body(),
+				btPivotA,
+				btPivotB));
+	} else {
+		btVector3 btPivotA;
+		G_TO_B(p_pos_a, btPivotA);
+		p2pConstraint = bulletnew(btPoint2PointConstraint(*p_body_a->get_bt_rigid_body(), btPivotA));
+	}
+
+	setup(p2pConstraint);
+}
+
+PinJointBullet::~PinJointBullet() {}
+
+void PinJointBullet::set_param(PhysicsServer::PinJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::PIN_JOINT_BIAS:
+			p2pConstraint->m_setting.m_tau = p_value;
+			break;
+		case PhysicsServer::PIN_JOINT_DAMPING:
+			p2pConstraint->m_setting.m_damping = p_value;
+			break;
+		case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
+			p2pConstraint->m_setting.m_impulseClamp = p_value;
+			break;
+	}
+}
+
+real_t PinJointBullet::get_param(PhysicsServer::PinJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer::PIN_JOINT_BIAS:
+			return p2pConstraint->m_setting.m_tau;
+		case PhysicsServer::PIN_JOINT_DAMPING:
+			return p2pConstraint->m_setting.m_damping;
+		case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP:
+			return p2pConstraint->m_setting.m_impulseClamp;
+		default:
+			WARN_PRINTS("This get parameter is not supported");
+			return 0;
+	}
+}
+
+void PinJointBullet::setPivotInA(const Vector3 &p_pos) {
+	btVector3 btVec;
+	G_TO_B(p_pos, btVec);
+	p2pConstraint->setPivotA(btVec);
+}
+
+void PinJointBullet::setPivotInB(const Vector3 &p_pos) {
+	btVector3 btVec;
+	G_TO_B(p_pos, btVec);
+	p2pConstraint->setPivotB(btVec);
+}
+
+Vector3 PinJointBullet::getPivotInA() {
+	btVector3 vec = p2pConstraint->getPivotInA();
+	Vector3 gVec;
+	B_TO_G(vec, gVec);
+	return gVec;
+}
+
+Vector3 PinJointBullet::getPivotInB() {
+	btVector3 vec = p2pConstraint->getPivotInB();
+	Vector3 gVec;
+	B_TO_G(vec, gVec);
+	return gVec;
+}

modules/bullet/pin_joint_bullet.h

@@ -0,0 +1,57 @@
+/*************************************************************************/
+/*  pin_joint_bullet.h                                                   */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef PIN_JOINT_BULLET_H
+#define PIN_JOINT_BULLET_H
+
+#include "joint_bullet.h"
+
+class RigidBodyBullet;
+
+class PinJointBullet : public JointBullet {
+	class btPoint2PointConstraint *p2pConstraint;
+
+public:
+	PinJointBullet(RigidBodyBullet *p_body_a, const Vector3 &p_pos_a, RigidBodyBullet *p_body_b, const Vector3 &p_pos_b);
+	~PinJointBullet();
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
+
+	void set_param(PhysicsServer::PinJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::PinJointParam p_param) const;
+
+	void setPivotInA(const Vector3 &p_pos);
+	void setPivotInB(const Vector3 &p_pos);
+
+	Vector3 getPivotInA();
+	Vector3 getPivotInB();
+};
+#endif

modules/bullet/register_types.cpp

@@ -0,0 +1,47 @@
+/*************************************************************************/
+/*  register_types.cpp                                                   */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "register_types.h"
+#include "bullet_physics_server.h"
+#include "class_db.h"
+
+PhysicsServer *_createBulletPhysicsCallback() {
+	return memnew(BulletPhysicsServer);
+}
+
+void register_bullet_types() {
+
+	PhysicsServerManager::register_server("Bullet", &_createBulletPhysicsCallback);
+	PhysicsServerManager::set_default_server("Bullet", 1);
+}
+
+void unregister_bullet_types() {
+}

modules/bullet/register_types.h

@@ -0,0 +1,37 @@
+/*************************************************************************/
+/*  register_types.h                                                     */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef REGISTER_BULLET_TYPES_H
+#define REGISTER_BULLET_TYPES_H
+
+void register_bullet_types();
+void unregister_bullet_types();
+#endif

modules/bullet/rid_bullet.h

@@ -0,0 +1,50 @@
+/*************************************************************************/
+/*  rid_bullet.h                                                         */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef RID_BULLET_H
+#define RID_BULLET_H
+
+#include "core/rid.h"
+
+class BulletPhysicsServer;
+
+class RIDBullet : public RID_Data {
+	RID self;
+	BulletPhysicsServer *physicsServer;
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+
+	_FORCE_INLINE_ void _set_physics_server(BulletPhysicsServer *p_physicsServer) { physicsServer = p_physicsServer; }
+	_FORCE_INLINE_ BulletPhysicsServer *get_physics_server() const { return physicsServer; }
+};
+#endif

modules/bullet/rigid_body_bullet.cpp

@@ -0,0 +1,1009 @@
+/*************************************************************************/
+/*  body_bullet.cpp                                                      */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "rigid_body_bullet.h"
+#include "BulletCollision/CollisionDispatch/btGhostObject.h"
+#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btBulletCollisionCommon.h"
+#include "btRayShape.h"
+#include "bullet_physics_server.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "godot_motion_state.h"
+#include "joint_bullet.h"
+#include <assert.h>
+
+BulletPhysicsDirectBodyState *BulletPhysicsDirectBodyState::singleton = NULL;
+
+Vector3 BulletPhysicsDirectBodyState::get_total_gravity() const {
+	Vector3 gVec;
+	B_TO_G(body->btBody->getGravity(), gVec);
+	return gVec;
+}
+
+float BulletPhysicsDirectBodyState::get_total_angular_damp() const {
+	return body->btBody->getAngularDamping();
+}
+
+float BulletPhysicsDirectBodyState::get_total_linear_damp() const {
+	return body->btBody->getLinearDamping();
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_center_of_mass() const {
+	Vector3 gVec;
+	B_TO_G(body->btBody->getCenterOfMassPosition(), gVec);
+	return gVec;
+}
+
+Basis BulletPhysicsDirectBodyState::get_principal_inertia_axes() const {
+	return Basis();
+}
+
+float BulletPhysicsDirectBodyState::get_inverse_mass() const {
+	return body->btBody->getInvMass();
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_inverse_inertia() const {
+	Vector3 gVec;
+	B_TO_G(body->btBody->getInvInertiaDiagLocal(), gVec);
+	return gVec;
+}
+
+Basis BulletPhysicsDirectBodyState::get_inverse_inertia_tensor() const {
+	Basis gInertia;
+	B_TO_G(body->btBody->getInvInertiaTensorWorld(), gInertia);
+	return gInertia;
+}
+
+void BulletPhysicsDirectBodyState::set_linear_velocity(const Vector3 &p_velocity) {
+	body->set_linear_velocity(p_velocity);
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void BulletPhysicsDirectBodyState::set_angular_velocity(const Vector3 &p_velocity) {
+	body->set_angular_velocity(p_velocity);
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void BulletPhysicsDirectBodyState::set_transform(const Transform &p_transform) {
+	body->set_transform(p_transform);
+}
+
+Transform BulletPhysicsDirectBodyState::get_transform() const {
+	return body->get_transform();
+}
+
+void BulletPhysicsDirectBodyState::add_force(const Vector3 &p_force, const Vector3 &p_pos) {
+	body->apply_force(p_force, p_pos);
+}
+
+void BulletPhysicsDirectBodyState::apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) {
+	body->apply_impulse(p_pos, p_j);
+}
+
+void BulletPhysicsDirectBodyState::apply_torque_impulse(const Vector3 &p_j) {
+	body->apply_torque_impulse(p_j);
+}
+
+void BulletPhysicsDirectBodyState::set_sleep_state(bool p_enable) {
+	body->set_activation_state(p_enable);
+}
+
+bool BulletPhysicsDirectBodyState::is_sleeping() const {
+	return !body->is_active();
+}
+
+int BulletPhysicsDirectBodyState::get_contact_count() const {
+	return body->collisionsCount;
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_contact_local_position(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].hitLocalLocation;
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_contact_local_normal(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].hitNormal;
+}
+
+int BulletPhysicsDirectBodyState::get_contact_local_shape(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].local_shape;
+}
+
+RID BulletPhysicsDirectBodyState::get_contact_collider(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].otherObject->get_self();
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_contact_collider_position(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].hitWorldLocation;
+}
+
+ObjectID BulletPhysicsDirectBodyState::get_contact_collider_id(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].otherObject->get_instance_id();
+}
+
+int BulletPhysicsDirectBodyState::get_contact_collider_shape(int p_contact_idx) const {
+	return body->collisions[p_contact_idx].other_object_shape;
+}
+
+Vector3 BulletPhysicsDirectBodyState::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	RigidBodyBullet::CollisionData &colDat = body->collisions[p_contact_idx];
+
+	btVector3 hitLocation;
+	G_TO_B(colDat.hitLocalLocation, hitLocation);
+
+	Vector3 velocityAtPoint;
+	B_TO_G(colDat.otherObject->get_bt_rigid_body()->getVelocityInLocalPoint(hitLocation), velocityAtPoint);
+
+	return velocityAtPoint;
+}
+
+PhysicsDirectSpaceState *BulletPhysicsDirectBodyState::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
+
+RigidBodyBullet::KinematicUtilities::KinematicUtilities(RigidBodyBullet *p_owner)
+	: m_owner(p_owner), m_margin(0.01) // Godot default margin 0.001
+{
+	m_ghostObject = bulletnew(btPairCachingGhostObject);
+
+	int clearedCurrentFlags = m_ghostObject->getCollisionFlags();
+	clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
+
+	m_ghostObject->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT);
+	m_ghostObject->setUserPointer(p_owner);
+	m_ghostObject->setUserIndex(TYPE_KINEMATIC_GHOST_BODY);
+
+	resetDefShape();
+}
+
+RigidBodyBullet::KinematicUtilities::~KinematicUtilities() {
+	just_delete_shapes(m_shapes.size()); // don't need to resize
+	bulletdelete(m_ghostObject);
+}
+
+void RigidBodyBullet::KinematicUtilities::resetDefShape() {
+	m_ghostObject->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+}
+
+void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
+	const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(m_owner->get_shapes_wrappers());
+	const int shapes_count = shapes_wrappers.size();
+
+	just_delete_shapes(shapes_count);
+
+	const CollisionObjectBullet::ShapeWrapper *shape_wrapper;
+
+	for (int i = shapes_count - 1; 0 <= i; --i) {
+		shape_wrapper = &shapes_wrappers[i];
+		if (!shape_wrapper->active) {
+			continue;
+		}
+		m_shapes[i].transform = shape_wrapper->transform;
+
+		btConvexShape *&kin_shape_ref = m_shapes[i].shape;
+
+		switch (shape_wrapper->shape->get_type()) {
+			case PhysicsServer::SHAPE_SPHERE: {
+				SphereShapeBullet *sphere = static_cast<SphereShapeBullet *>(shape_wrapper->shape);
+				kin_shape_ref = ShapeBullet::create_shape_sphere(sphere->get_radius() * m_owner->body_scale[0] + m_margin);
+				break;
+			}
+			case PhysicsServer::SHAPE_BOX: {
+				BoxShapeBullet *box = static_cast<BoxShapeBullet *>(shape_wrapper->shape);
+				kin_shape_ref = ShapeBullet::create_shape_box((box->get_half_extents() * m_owner->body_scale) + btVector3(m_margin, m_margin, m_margin));
+				break;
+			}
+			case PhysicsServer::SHAPE_CAPSULE: {
+				CapsuleShapeBullet *capsule = static_cast<CapsuleShapeBullet *>(shape_wrapper->shape);
+				kin_shape_ref = ShapeBullet::create_shape_capsule(capsule->get_radius() * m_owner->body_scale[0] + m_margin, capsule->get_height() * m_owner->body_scale[1] + m_margin);
+				break;
+			}
+			case PhysicsServer::SHAPE_CONVEX_POLYGON: {
+				ConvexPolygonShapeBullet *godot_convex = static_cast<ConvexPolygonShapeBullet *>(shape_wrapper->shape);
+				kin_shape_ref = ShapeBullet::create_shape_convex(godot_convex->vertices);
+				kin_shape_ref->setLocalScaling(m_owner->body_scale + btVector3(m_margin, m_margin, m_margin));
+				break;
+			}
+			case PhysicsServer::SHAPE_RAY: {
+				RayShapeBullet *godot_ray = static_cast<RayShapeBullet *>(shape_wrapper->shape);
+				kin_shape_ref = ShapeBullet::create_shape_ray(godot_ray->length * m_owner->body_scale[1] + m_margin);
+				break;
+			}
+			default:
+				WARN_PRINT("This shape is not supported to be kinematic!");
+				kin_shape_ref = NULL;
+		}
+	}
+}
+
+void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) {
+	for (int i = m_shapes.size() - 1; 0 <= i; --i) {
+		if (m_shapes[i].shape) {
+			bulletdelete(m_shapes[i].shape);
+		}
+	}
+	m_shapes.resize(new_size);
+}
+
+RigidBodyBullet::RigidBodyBullet()
+	: RigidCollisionObjectBullet(CollisionObjectBullet::TYPE_RIGID_BODY),
+	  kinematic_utilities(NULL),
+	  gravity_scale(1),
+	  mass(1),
+	  linearDamp(0),
+	  angularDamp(0),
+	  can_sleep(true),
+	  force_integration_callback(NULL),
+	  isTransformChanged(false),
+	  maxCollisionsDetection(0),
+	  collisionsCount(0),
+	  maxAreasWhereIam(10),
+	  areaWhereIamCount(0),
+	  countGravityPointSpaces(0),
+	  isScratchedSpaceOverrideModificator(false) {
+
+	godotMotionState = bulletnew(GodotMotionState(this));
+
+	// Initial properties
+	const btVector3 localInertia(0, 0, 0);
+	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, compoundShape, localInertia);
+
+	btBody = bulletnew(btRigidBody(cInfo));
+	setupBulletCollisionObject(btBody);
+
+	set_mode(PhysicsServer::BODY_MODE_RIGID);
+	set_axis_lock(PhysicsServer::BODY_AXIS_LOCK_DISABLED);
+
+	areasWhereIam.resize(maxAreasWhereIam);
+	for (int i = areasWhereIam.size() - 1; 0 <= i; --i) {
+		areasWhereIam[i] = NULL;
+	}
+}
+
+RigidBodyBullet::~RigidBodyBullet() {
+	bulletdelete(godotMotionState);
+
+	if (force_integration_callback)
+		memdelete(force_integration_callback);
+
+	destroy_kinematic_utilities();
+}
+
+void RigidBodyBullet::init_kinematic_utilities() {
+	kinematic_utilities = memnew(KinematicUtilities(this));
+}
+
+void RigidBodyBullet::destroy_kinematic_utilities() {
+	if (kinematic_utilities) {
+		memdelete(kinematic_utilities);
+		kinematic_utilities = NULL;
+	}
+}
+
+void RigidBodyBullet::reload_body() {
+	if (space) {
+		space->remove_rigid_body(this);
+		space->add_rigid_body(this);
+	}
+}
+
+void RigidBodyBullet::set_space(SpaceBullet *p_space) {
+	// Clear the old space if there is one
+	if (space) {
+		isTransformChanged = false;
+
+		// Remove all eventual constraints
+		assert_no_constraints();
+
+		// Remove this object form the physics world
+		space->remove_rigid_body(this);
+	}
+
+	space = p_space;
+
+	if (space) {
+		space->add_rigid_body(this);
+	}
+}
+
+void RigidBodyBullet::dispatch_callbacks() {
+	/// The check isTransformChanged is necessary in order to call integrated forces only when the first transform is sent
+	if (btBody->isActive() && force_integration_callback && isTransformChanged) {
+
+		BulletPhysicsDirectBodyState *bodyDirect = BulletPhysicsDirectBodyState::get_singleton(this);
+
+		Variant variantBodyDirect = bodyDirect;
+
+		Object *obj = ObjectDB::get_instance(force_integration_callback->id);
+		if (!obj) {
+			// Remove integration callback
+			set_force_integration_callback(0, StringName());
+		} else {
+			const Variant *vp[2] = { &variantBodyDirect, &force_integration_callback->udata };
+
+			Variant::CallError responseCallError;
+			int argc = (force_integration_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
+			obj->call(force_integration_callback->method, vp, argc, responseCallError);
+		}
+	}
+
+	if (isScratchedSpaceOverrideModificator || 0 < countGravityPointSpaces) {
+		isScratchedSpaceOverrideModificator = false;
+		reload_space_override_modificator();
+	}
+
+	/// Lock axis
+	btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor());
+	btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor());
+}
+
+void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
+
+	if (force_integration_callback) {
+		memdelete(force_integration_callback);
+		force_integration_callback = NULL;
+	}
+
+	if (p_id != 0) {
+		force_integration_callback = memnew(ForceIntegrationCallback);
+		force_integration_callback->id = p_id;
+		force_integration_callback->method = p_method;
+		force_integration_callback->udata = p_udata;
+	}
+}
+
+void RigidBodyBullet::scratch() {
+	isTransformChanged = true;
+}
+
+void RigidBodyBullet::scratch_space_override_modificator() {
+	isScratchedSpaceOverrideModificator = true;
+}
+
+void RigidBodyBullet::on_collision_filters_change() {
+	if (space) {
+		space->reload_collision_filters(this);
+	}
+}
+
+void RigidBodyBullet::on_collision_checker_start() {
+	collisionsCount = 0;
+}
+
+bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, int p_other_shape_index, int p_local_shape_index) {
+
+	if (collisionsCount >= maxCollisionsDetection) {
+		return false;
+	}
+
+	CollisionData &cd = collisions[collisionsCount];
+	cd.hitLocalLocation = p_hitLocalLocation;
+	cd.otherObject = p_otherObject;
+	cd.hitWorldLocation = p_hitWorldLocation;
+	cd.hitNormal = p_hitNormal;
+	cd.other_object_shape = p_other_shape_index;
+	cd.local_shape = p_local_shape_index;
+
+	++collisionsCount;
+	return true;
+}
+
+void RigidBodyBullet::assert_no_constraints() {
+	if (btBody->getNumConstraintRefs()) {
+		WARN_PRINT("A body with a joints is destroyed. Please check the implementation in order to destroy the joint before the body.");
+	}
+	/*for(int i = btBody->getNumConstraintRefs()-1; 0<=i; --i){
+        btTypedConstraint* btConst = btBody->getConstraintRef(i);
+        JointBullet* joint = static_cast<JointBullet*>( btConst->getUserConstraintPtr() );
+        space->removeConstraint(joint);
+    }*/
+}
+
+void RigidBodyBullet::set_activation_state(bool p_active) {
+	if (p_active) {
+		btBody->setActivationState(ACTIVE_TAG);
+	} else {
+		btBody->setActivationState(WANTS_DEACTIVATION);
+	}
+}
+
+bool RigidBodyBullet::is_active() const {
+	return btBody->isActive();
+}
+
+void RigidBodyBullet::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE:
+			btBody->setRestitution(p_value);
+			break;
+		case PhysicsServer::BODY_PARAM_FRICTION:
+			btBody->setFriction(p_value);
+			break;
+		case PhysicsServer::BODY_PARAM_MASS: {
+			ERR_FAIL_COND(p_value < 0);
+			mass = p_value;
+			_internal_set_mass(p_value);
+			break;
+		}
+		case PhysicsServer::BODY_PARAM_LINEAR_DAMP:
+			linearDamp = p_value;
+			btBody->setDamping(linearDamp, angularDamp);
+			break;
+		case PhysicsServer::BODY_PARAM_ANGULAR_DAMP:
+			angularDamp = p_value;
+			btBody->setDamping(linearDamp, angularDamp);
+			break;
+		case PhysicsServer::BODY_PARAM_GRAVITY_SCALE:
+			gravity_scale = p_value;
+			/// The Bullet gravity will be is set by reload_space_override_modificator
+			scratch_space_override_modificator();
+			break;
+		default:
+			WARN_PRINTS("Parameter " + itos(p_param) + " not supported by bullet. Value: " + itos(p_value));
+	}
+}
+
+real_t RigidBodyBullet::get_param(PhysicsServer::BodyParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE:
+			return btBody->getRestitution();
+		case PhysicsServer::BODY_PARAM_FRICTION:
+			return btBody->getFriction();
+		case PhysicsServer::BODY_PARAM_MASS: {
+			const btScalar invMass = btBody->getInvMass();
+			return 0 == invMass ? 0 : 1 / invMass;
+		}
+		case PhysicsServer::BODY_PARAM_LINEAR_DAMP:
+			return linearDamp;
+		case PhysicsServer::BODY_PARAM_ANGULAR_DAMP:
+			return angularDamp;
+		case PhysicsServer::BODY_PARAM_GRAVITY_SCALE:
+			return gravity_scale;
+		default:
+			WARN_PRINTS("Parameter " + itos(p_param) + " not supported by bullet");
+			return 0;
+	}
+}
+
+void RigidBodyBullet::set_mode(PhysicsServer::BodyMode p_mode) {
+	// This is necessary to block force_integration untile next move
+	isTransformChanged = false;
+	destroy_kinematic_utilities();
+	// The mode change is relevant to its mass
+	switch (p_mode) {
+		case PhysicsServer::BODY_MODE_KINEMATIC:
+			mode = PhysicsServer::BODY_MODE_KINEMATIC;
+			set_axis_lock(axis_lock); // Reload axis lock
+			_internal_set_mass(0);
+			init_kinematic_utilities();
+			break;
+		case PhysicsServer::BODY_MODE_STATIC:
+			mode = PhysicsServer::BODY_MODE_STATIC;
+			set_axis_lock(axis_lock); // Reload axis lock
+			_internal_set_mass(0);
+			break;
+		case PhysicsServer::BODY_MODE_RIGID: {
+			mode = PhysicsServer::BODY_MODE_RIGID;
+			set_axis_lock(axis_lock); // Reload axis lock
+			_internal_set_mass(0 == mass ? 1 : mass);
+			break;
+		}
+		case PhysicsServer::BODY_MODE_CHARACTER: {
+			mode = PhysicsServer::BODY_MODE_CHARACTER;
+			set_axis_lock(axis_lock); // Reload axis lock
+			_internal_set_mass(0 == mass ? 1 : mass);
+			break;
+		}
+	}
+
+	btBody->setAngularVelocity(btVector3(0, 0, 0));
+	btBody->setLinearVelocity(btVector3(0, 0, 0));
+}
+PhysicsServer::BodyMode RigidBodyBullet::get_mode() const {
+	return mode;
+}
+
+void RigidBodyBullet::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) {
+
+	switch (p_state) {
+		case PhysicsServer::BODY_STATE_TRANSFORM:
+			set_transform(p_variant);
+			break;
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY:
+			set_linear_velocity(p_variant);
+			break;
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY:
+			set_angular_velocity(p_variant);
+			break;
+		case PhysicsServer::BODY_STATE_SLEEPING:
+			set_activation_state(!bool(p_variant));
+			break;
+		case PhysicsServer::BODY_STATE_CAN_SLEEP:
+			can_sleep = bool(p_variant);
+			if (!can_sleep) {
+				// Can't sleep
+				btBody->forceActivationState(DISABLE_DEACTIVATION);
+			}
+			break;
+	}
+}
+
+Variant RigidBodyBullet::get_state(PhysicsServer::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer::BODY_STATE_TRANSFORM:
+			return get_transform();
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY:
+			return get_linear_velocity();
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY:
+			return get_angular_velocity();
+		case PhysicsServer::BODY_STATE_SLEEPING:
+			return !is_active();
+		case PhysicsServer::BODY_STATE_CAN_SLEEP:
+			return can_sleep;
+		default:
+			WARN_PRINTS("This state " + itos(p_state) + " is not supported by Bullet");
+			return Variant();
+	}
+}
+
+void RigidBodyBullet::apply_central_impulse(const Vector3 &p_impulse) {
+	btVector3 btImpu;
+	G_TO_B(p_impulse, btImpu);
+	btBody->activate();
+	btBody->applyCentralImpulse(btImpu);
+}
+
+void RigidBodyBullet::apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse) {
+	btVector3 btImpu;
+	btVector3 btPos;
+	G_TO_B(p_impulse, btImpu);
+	G_TO_B(p_pos, btPos);
+	btBody->activate();
+	btBody->applyImpulse(btImpu, btPos);
+}
+
+void RigidBodyBullet::apply_torque_impulse(const Vector3 &p_impulse) {
+	btVector3 btImp;
+	G_TO_B(p_impulse, btImp);
+	btBody->activate();
+	btBody->applyTorqueImpulse(btImp);
+}
+
+void RigidBodyBullet::apply_force(const Vector3 &p_force, const Vector3 &p_pos) {
+	btVector3 btForce;
+	btVector3 btPos;
+	G_TO_B(p_force, btForce);
+	G_TO_B(p_pos, btPos);
+	btBody->activate();
+	btBody->applyForce(btForce, btPos);
+}
+
+void RigidBodyBullet::apply_central_force(const Vector3 &p_force) {
+	btVector3 btForce;
+	G_TO_B(p_force, btForce);
+	btBody->activate();
+	btBody->applyCentralForce(btForce);
+}
+
+void RigidBodyBullet::apply_torque(const Vector3 &p_torque) {
+	btVector3 btTorq;
+	G_TO_B(p_torque, btTorq);
+	btBody->activate();
+	btBody->applyTorque(btTorq);
+}
+
+void RigidBodyBullet::set_applied_force(const Vector3 &p_force) {
+	btVector3 btVec = btBody->getTotalTorque();
+
+	btBody->activate();
+
+	btBody->clearForces();
+	btBody->applyTorque(btVec);
+
+	G_TO_B(p_force, btVec);
+	btBody->applyCentralForce(btVec);
+}
+
+Vector3 RigidBodyBullet::get_applied_force() const {
+	Vector3 gTotForc;
+	B_TO_G(btBody->getTotalForce(), gTotForc);
+	return gTotForc;
+}
+
+void RigidBodyBullet::set_applied_torque(const Vector3 &p_torque) {
+	btVector3 btVec = btBody->getTotalForce();
+
+	btBody->activate();
+
+	btBody->clearForces();
+	btBody->applyCentralForce(btVec);
+
+	G_TO_B(p_torque, btVec);
+	btBody->applyTorque(btVec);
+}
+
+Vector3 RigidBodyBullet::get_applied_torque() const {
+	Vector3 gTotTorq;
+	B_TO_G(btBody->getTotalTorque(), gTotTorq);
+	return gTotTorq;
+}
+
+void RigidBodyBullet::set_axis_lock(PhysicsServer::BodyAxisLock p_lock) {
+	axis_lock = p_lock;
+
+	if (PhysicsServer::BODY_AXIS_LOCK_DISABLED == axis_lock) {
+		btBody->setLinearFactor(btVector3(1., 1., 1.));
+		btBody->setAngularFactor(btVector3(1., 1., 1.));
+	} else if (PhysicsServer::BODY_AXIS_LOCK_X == axis_lock) {
+		btBody->setLinearFactor(btVector3(0., 1., 1.));
+		btBody->setAngularFactor(btVector3(1., 0., 0.));
+	} else if (PhysicsServer::BODY_AXIS_LOCK_Y == axis_lock) {
+		btBody->setLinearFactor(btVector3(1., 0., 1.));
+		btBody->setAngularFactor(btVector3(0., 1., 0.));
+	} else if (PhysicsServer::BODY_AXIS_LOCK_Z == axis_lock) {
+		btBody->setLinearFactor(btVector3(1., 1., 0.));
+		btBody->setAngularFactor(btVector3(0., 0., 1.));
+	}
+
+	if (PhysicsServer::BODY_MODE_CHARACTER == mode) {
+		/// When character lock angular
+		btBody->setAngularFactor(btVector3(0., 0., 0.));
+	}
+}
+
+PhysicsServer::BodyAxisLock RigidBodyBullet::get_axis_lock() const {
+	btVector3 vec = btBody->getLinearFactor();
+	if (0. == vec.x()) {
+		return PhysicsServer::BODY_AXIS_LOCK_X;
+	} else if (0. == vec.y()) {
+		return PhysicsServer::BODY_AXIS_LOCK_Y;
+	} else if (0. == vec.z()) {
+		return PhysicsServer::BODY_AXIS_LOCK_Z;
+	} else {
+		return PhysicsServer::BODY_AXIS_LOCK_DISABLED;
+	}
+}
+
+void RigidBodyBullet::set_continuous_collision_detection(bool p_enable) {
+	if (p_enable) {
+		// This threshold enable CCD if the object moves more than
+		// 1 meter in one simulation frame
+		btBody->setCcdMotionThreshold(1);
+
+		/// Calculate using the rule writte below the CCD swept sphere radius
+		///     CCD works on an embedded sphere of radius, make sure this radius
+		///     is embedded inside the convex objects, preferably smaller:
+		///     for an object of dimentions 1 meter, try 0.2
+		btVector3 center;
+		btScalar radius;
+		btBody->getCollisionShape()->getBoundingSphere(center, radius);
+		btBody->setCcdSweptSphereRadius(radius * 0.2);
+	} else {
+		btBody->setCcdMotionThreshold(0.);
+		btBody->setCcdSweptSphereRadius(0.);
+	}
+}
+
+bool RigidBodyBullet::is_continuous_collision_detection_enabled() const {
+	return 0. != btBody->getCcdMotionThreshold();
+}
+
+void RigidBodyBullet::set_linear_velocity(const Vector3 &p_velocity) {
+	btVector3 btVec;
+	G_TO_B(p_velocity, btVec);
+	btBody->activate();
+	btBody->setLinearVelocity(btVec);
+}
+
+Vector3 RigidBodyBullet::get_linear_velocity() const {
+	Vector3 gVec;
+	B_TO_G(btBody->getLinearVelocity(), gVec);
+	return gVec;
+}
+
+void RigidBodyBullet::set_angular_velocity(const Vector3 &p_velocity) {
+	btVector3 btVec;
+	G_TO_B(p_velocity, btVec);
+	btBody->activate();
+	btBody->setAngularVelocity(btVec);
+}
+
+Vector3 RigidBodyBullet::get_angular_velocity() const {
+	Vector3 gVec;
+	B_TO_G(btBody->getAngularVelocity(), gVec);
+	return gVec;
+}
+
+void RigidBodyBullet::set_transform__bullet(const btTransform &p_global_transform) {
+	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
+		// The kinematic use MotionState class
+		godotMotionState->moveBody(p_global_transform);
+	}
+	btBody->setWorldTransform(p_global_transform);
+}
+
+const btTransform &RigidBodyBullet::get_transform__bullet() const {
+	if (is_static()) {
+
+		return RigidCollisionObjectBullet::get_transform__bullet();
+	} else {
+
+		return godotMotionState->getCurrentWorldTransform();
+	}
+}
+
+void RigidBodyBullet::on_shapes_changed() {
+	RigidCollisionObjectBullet::on_shapes_changed();
+
+	const btScalar invMass = btBody->getInvMass();
+	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
+
+	btVector3 inertia;
+	btBody->getCollisionShape()->calculateLocalInertia(mass, inertia);
+	btBody->setMassProps(mass, inertia);
+	btBody->updateInertiaTensor();
+
+	reload_kinematic_shapes();
+
+	reload_body();
+}
+
+void RigidBodyBullet::on_enter_area(AreaBullet *p_area) {
+	/// Add this area to the array in an ordered way
+	++areaWhereIamCount;
+	if (areaWhereIamCount >= maxAreasWhereIam) {
+		--areaWhereIamCount;
+		return;
+	}
+	for (int i = 0; i < areaWhereIamCount; ++i) {
+
+		if (NULL == areasWhereIam[i]) {
+			// This area has the highest priority
+			areasWhereIam[i] = p_area;
+			break;
+		} else {
+			if (areasWhereIam[i]->get_spOv_priority() > p_area->get_spOv_priority()) {
+				// The position was found, just shift all elements
+				for (int j = i; j < areaWhereIamCount; ++j) {
+					areasWhereIam[j + 1] = areasWhereIam[j];
+				}
+				areasWhereIam[i] = p_area;
+				break;
+			}
+		}
+	}
+	if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
+		scratch_space_override_modificator();
+	}
+
+	if (p_area->is_spOv_gravityPoint()) {
+		++countGravityPointSpaces;
+		assert(0 < countGravityPointSpaces);
+	}
+}
+
+void RigidBodyBullet::on_exit_area(AreaBullet *p_area) {
+	RigidCollisionObjectBullet::on_exit_area(p_area);
+	/// Remove this area and keep the order
+	/// N.B. Since I don't want resize the array I can't use the "erase" function
+	bool wasTheAreaFound = false;
+	for (int i = 0; i < areaWhereIamCount; ++i) {
+		if (p_area == areasWhereIam[i]) {
+			// The area was fount, just shift down all elements
+			for (int j = i; j < areaWhereIamCount; ++j) {
+				areasWhereIam[j] = areasWhereIam[j + 1];
+			}
+			wasTheAreaFound = true;
+			break;
+		}
+	}
+	if (wasTheAreaFound) {
+		if (p_area->is_spOv_gravityPoint()) {
+			--countGravityPointSpaces;
+			assert(0 <= countGravityPointSpaces);
+		}
+
+		--areaWhereIamCount;
+		areasWhereIam[areaWhereIamCount] = NULL; // Even if this is not required, I clear the last element to be safe
+		if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
+			scratch_space_override_modificator();
+		}
+	}
+}
+
+void RigidBodyBullet::reload_space_override_modificator() {
+
+	Vector3 newGravity(space->get_gravity_direction() * space->get_gravity_magnitude());
+	real_t newLinearDamp(linearDamp);
+	real_t newAngularDamp(angularDamp);
+
+	AreaBullet *currentArea;
+	// Variable used to calculate new gravity for gravity point areas, it is pointed by currentGravity pointer
+	Vector3 support_gravity(0, 0, 0);
+
+	int countCombined(0);
+	for (int i = areaWhereIamCount - 1; 0 <= i; --i) {
+
+		currentArea = areasWhereIam[i];
+
+		if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED == currentArea->get_spOv_mode()) {
+			continue;
+		}
+
+		/// Here is calculated the gravity
+		if (currentArea->is_spOv_gravityPoint()) {
+
+			/// It calculates the direction of new gravity
+			support_gravity = currentArea->get_transform().xform(currentArea->get_spOv_gravityVec()) - get_transform().get_origin();
+			real_t distanceMag = support_gravity.length();
+			// Normalized in this way to avoid the double call of function "length()"
+			if (distanceMag == 0) {
+				support_gravity.x = 0;
+				support_gravity.y = 0;
+				support_gravity.z = 0;
+			} else {
+				support_gravity.x /= distanceMag;
+				support_gravity.y /= distanceMag;
+				support_gravity.z /= distanceMag;
+			}
+
+			/// Here is calculated the final gravity
+			if (currentArea->get_spOv_gravityPointDistanceScale() > 0) {
+				// Scaled gravity by distance
+				support_gravity *= currentArea->get_spOv_gravityMag() / Math::pow(distanceMag * currentArea->get_spOv_gravityPointDistanceScale() + 1, 2);
+			} else {
+				// Unscaled gravity
+				support_gravity *= currentArea->get_spOv_gravityMag();
+			}
+		} else {
+			support_gravity = currentArea->get_spOv_gravityVec() * currentArea->get_spOv_gravityMag();
+		}
+
+		switch (currentArea->get_spOv_mode()) {
+			///case PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED:
+			/// This area does not affect gravity/damp. These are generally areas
+			/// that exist only to detect collisions, and objects entering or exiting them.
+			///    break;
+			case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE:
+				/// This area adds its gravity/damp values to whatever has been
+				/// calculated so far. This way, many overlapping areas can combine
+				/// their physics to make interesting
+				newGravity += support_gravity;
+				newLinearDamp += currentArea->get_spOv_linearDamp();
+				newAngularDamp += currentArea->get_spOv_angularDamp();
+				++countCombined;
+				break;
+			case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE:
+				/// This area adds its gravity/damp values to whatever has been calculated
+				/// so far. Then stops taking into account the rest of the areas, even the
+				/// default one.
+				newGravity += support_gravity;
+				newLinearDamp += currentArea->get_spOv_linearDamp();
+				newAngularDamp += currentArea->get_spOv_angularDamp();
+				++countCombined;
+				goto endAreasCycle;
+			case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE:
+				/// This area replaces any gravity/damp, even the default one, and
+				/// stops taking into account the rest of the areas.
+				newGravity = support_gravity;
+				newLinearDamp = currentArea->get_spOv_linearDamp();
+				newAngularDamp = currentArea->get_spOv_angularDamp();
+				countCombined = 1;
+				goto endAreasCycle;
+			case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE:
+				/// This area replaces any gravity/damp calculated so far, but keeps
+				/// calculating the rest of the areas, down to the default one.
+				newGravity = support_gravity;
+				newLinearDamp = currentArea->get_spOv_linearDamp();
+				newAngularDamp = currentArea->get_spOv_angularDamp();
+				countCombined = 1;
+				break;
+		}
+	}
+endAreasCycle:
+
+	if (1 < countCombined) {
+		newGravity /= countCombined;
+		newLinearDamp /= countCombined;
+		newAngularDamp /= countCombined;
+	}
+
+	btVector3 newBtGravity;
+	G_TO_B(newGravity * gravity_scale, newBtGravity);
+
+	btBody->setGravity(newBtGravity);
+	btBody->setDamping(newLinearDamp, newAngularDamp);
+}
+
+void RigidBodyBullet::reload_kinematic_shapes() {
+	if (!kinematic_utilities) {
+		return;
+	}
+	kinematic_utilities->copyAllOwnerShapes();
+}
+
+void RigidBodyBullet::_internal_set_mass(real_t p_mass) {
+
+	btVector3 localInertia(0, 0, 0);
+
+	int clearedCurrentFlags = btBody->getCollisionFlags();
+	clearedCurrentFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_CHARACTER_OBJECT);
+
+	// Rigidbody is dynamic if and only if mass is non Zero, otherwise static
+	const bool isDynamic = p_mass != 0.f;
+	if (isDynamic) {
+
+		ERR_FAIL_COND(PhysicsServer::BODY_MODE_RIGID != mode && PhysicsServer::BODY_MODE_CHARACTER != mode);
+
+		m_isStatic = false;
+		compoundShape->calculateLocalInertia(p_mass, localInertia);
+
+		if (PhysicsServer::BODY_MODE_RIGID == mode) {
+
+			btBody->setCollisionFlags(clearedCurrentFlags); // Just set the flags without Kin and Static
+		} else {
+
+			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_CHARACTER_OBJECT);
+		}
+
+		if (can_sleep) {
+			btBody->forceActivationState(ACTIVE_TAG); // ACTIVE_TAG 1
+		} else {
+			btBody->forceActivationState(DISABLE_DEACTIVATION); // DISABLE_DEACTIVATION 4
+		}
+	} else {
+
+		ERR_FAIL_COND(PhysicsServer::BODY_MODE_STATIC != mode && PhysicsServer::BODY_MODE_KINEMATIC != mode);
+
+		m_isStatic = true;
+		if (PhysicsServer::BODY_MODE_STATIC == mode) {
+
+			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_STATIC_OBJECT);
+		} else {
+
+			btBody->setCollisionFlags(clearedCurrentFlags | btCollisionObject::CF_KINEMATIC_OBJECT);
+			set_transform__bullet(btBody->getWorldTransform()); // Set current Transform using kinematic method
+		}
+		btBody->forceActivationState(DISABLE_SIMULATION); // DISABLE_SIMULATION 5
+	}
+
+	btBody->setMassProps(p_mass, localInertia);
+	btBody->updateInertiaTensor();
+
+	reload_body();
+}

modules/bullet/rigid_body_bullet.h

@@ -0,0 +1,304 @@
+/*************************************************************************/
+/*  body_bullet.h                                                        */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BODYBULLET_H
+#define BODYBULLET_H
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "LinearMath/btTransform.h"
+#include "collision_object_bullet.h"
+#include "space_bullet.h"
+
+class AreaBullet;
+class SpaceBullet;
+class btRigidBody;
+class GodotMotionState;
+class BulletPhysicsDirectBodyState;
+
+/// This class could be used in multi thread with few changes but currently
+/// is setted to be only in one single thread.
+///
+/// In the system there is only one object at a time that manage all bodies and is
+/// created by BulletPhysicsServer and is held by the "singleton" variable of this class
+/// Each time something require it, the body must be setted again.
+class BulletPhysicsDirectBodyState : public PhysicsDirectBodyState {
+	GDCLASS(BulletPhysicsDirectBodyState, PhysicsDirectBodyState)
+
+	static BulletPhysicsDirectBodyState *singleton;
+
+public:
+	/// This class avoid the creation of more object of this class
+	static void initSingleton() {
+		if (!singleton) {
+			singleton = memnew(BulletPhysicsDirectBodyState);
+		}
+	}
+
+	static void destroySingleton() {
+		memdelete(singleton);
+		singleton = NULL;
+	}
+
+	static void singleton_setDeltaTime(real_t p_deltaTime) {
+		singleton->deltaTime = p_deltaTime;
+	}
+
+	static BulletPhysicsDirectBodyState *get_singleton(RigidBodyBullet *p_body) {
+		singleton->body = p_body;
+		return singleton;
+	}
+
+public:
+	RigidBodyBullet *body;
+	real_t deltaTime;
+
+private:
+	BulletPhysicsDirectBodyState() {}
+
+public:
+	virtual Vector3 get_total_gravity() const;
+	virtual float get_total_angular_damp() const;
+	virtual float get_total_linear_damp() const;
+
+	virtual Vector3 get_center_of_mass() const;
+	virtual Basis get_principal_inertia_axes() const;
+	// get the mass
+	virtual float get_inverse_mass() const;
+	// get density of this body space
+	virtual Vector3 get_inverse_inertia() const;
+	// get density of this body space
+	virtual Basis get_inverse_inertia_tensor() const;
+
+	virtual void set_linear_velocity(const Vector3 &p_velocity);
+	virtual Vector3 get_linear_velocity() const;
+
+	virtual void set_angular_velocity(const Vector3 &p_velocity);
+	virtual Vector3 get_angular_velocity() const;
+
+	virtual void set_transform(const Transform &p_transform);
+	virtual Transform get_transform() const;
+
+	virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos);
+	virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j);
+	virtual void apply_torque_impulse(const Vector3 &p_j);
+
+	virtual void set_sleep_state(bool p_enable);
+	virtual bool is_sleeping() const;
+
+	virtual int get_contact_count() const;
+
+	virtual Vector3 get_contact_local_position(int p_contact_idx) const;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const;
+	virtual int get_contact_local_shape(int p_contact_idx) const;
+
+	virtual RID get_contact_collider(int p_contact_idx) const;
+	virtual Vector3 get_contact_collider_position(int p_contact_idx) const;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const;
+	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const;
+
+	virtual real_t get_step() const { return deltaTime; }
+	virtual void integrate_forces() {
+		// Skip the execution of this function
+	}
+
+	virtual PhysicsDirectSpaceState *get_space_state();
+};
+
+class RigidBodyBullet : public RigidCollisionObjectBullet {
+
+public:
+	struct CollisionData {
+		RigidBodyBullet *otherObject;
+		int other_object_shape;
+		int local_shape;
+		Vector3 hitLocalLocation;
+		Vector3 hitWorldLocation;
+		Vector3 hitNormal;
+	};
+
+	struct ForceIntegrationCallback {
+		ObjectID id;
+		StringName method;
+		Variant udata;
+	};
+
+	/// Used to hold shapes
+	struct KinematicShape {
+		class btConvexShape *shape;
+		btTransform transform;
+
+		KinematicShape()
+			: shape(NULL) {}
+		const bool is_active() const { return shape; }
+	};
+
+	struct KinematicUtilities {
+		RigidBodyBullet *m_owner;
+		btScalar m_margin;
+		btManifoldArray m_manifoldArray; ///keep track of the contact manifolds
+		class btPairCachingGhostObject *m_ghostObject;
+		Vector<KinematicShape> m_shapes;
+
+		KinematicUtilities(RigidBodyBullet *p_owner);
+		~KinematicUtilities();
+
+		/// Used to set the default shape to ghost
+		void resetDefShape();
+		void copyAllOwnerShapes();
+
+	private:
+		void just_delete_shapes(int new_size);
+	};
+
+private:
+	friend class BulletPhysicsDirectBodyState;
+
+	// This is required only for Kinematic movement
+	KinematicUtilities *kinematic_utilities;
+
+	PhysicsServer::BodyMode mode;
+	PhysicsServer::BodyAxisLock axis_lock;
+	GodotMotionState *godotMotionState;
+	btRigidBody *btBody;
+	real_t mass;
+	real_t gravity_scale;
+	real_t linearDamp;
+	real_t angularDamp;
+	bool can_sleep;
+
+	Vector<CollisionData> collisions;
+	// these parameters are used to avoid vector resize
+	int maxCollisionsDetection;
+	int collisionsCount;
+
+	Vector<AreaBullet *> areasWhereIam;
+	// these parameters are used to avoid vector resize
+	int maxAreasWhereIam;
+	int areaWhereIamCount;
+	// Used to know if the area is used as gravity point
+	int countGravityPointSpaces;
+	bool isScratchedSpaceOverrideModificator;
+
+	bool isTransformChanged;
+
+	ForceIntegrationCallback *force_integration_callback;
+
+public:
+	RigidBodyBullet();
+	~RigidBodyBullet();
+
+	void init_kinematic_utilities();
+	void destroy_kinematic_utilities();
+	_FORCE_INLINE_ class KinematicUtilities *get_kinematic_utilities() const { return kinematic_utilities; }
+
+	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
+
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
+
+	virtual void dispatch_callbacks();
+	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
+	void scratch();
+	void scratch_space_override_modificator();
+
+	virtual void on_collision_filters_change();
+	virtual void on_collision_checker_start();
+	void set_max_collisions_detection(int p_maxCollisionsDetection) {
+		maxCollisionsDetection = p_maxCollisionsDetection;
+		collisions.resize(p_maxCollisionsDetection);
+		collisionsCount = 0;
+	}
+	int get_max_collisions_detection() {
+		return maxCollisionsDetection;
+	}
+
+	bool can_add_collision() { return collisionsCount < maxCollisionsDetection; }
+	bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, int p_other_shape_index, int p_local_shape_index);
+
+	void assert_no_constraints();
+
+	void set_activation_state(bool p_active);
+	bool is_active() const;
+
+	void set_param(PhysicsServer::BodyParameter p_param, real_t);
+	real_t get_param(PhysicsServer::BodyParameter p_param) const;
+
+	void set_mode(PhysicsServer::BodyMode p_mode);
+	PhysicsServer::BodyMode get_mode() const;
+
+	void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant);
+	Variant get_state(PhysicsServer::BodyState p_state) const;
+
+	void apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse);
+	void apply_central_impulse(const Vector3 &p_force);
+	void apply_torque_impulse(const Vector3 &p_impulse);
+
+	void apply_force(const Vector3 &p_force, const Vector3 &p_pos);
+	void apply_central_force(const Vector3 &p_force);
+	void apply_torque(const Vector3 &p_force);
+
+	void set_applied_force(const Vector3 &p_force);
+	Vector3 get_applied_force() const;
+	void set_applied_torque(const Vector3 &p_torque);
+	Vector3 get_applied_torque() const;
+
+	void set_axis_lock(PhysicsServer::BodyAxisLock p_lock);
+	PhysicsServer::BodyAxisLock get_axis_lock() const;
+
+	/// Doc:
+	/// http://www.bulletphysics.org/mediawiki-1.5.8/index.php?title=Anti_tunneling_by_Motion_Clamping
+	void set_continuous_collision_detection(bool p_enable);
+	bool is_continuous_collision_detection_enabled() const;
+
+	void set_linear_velocity(const Vector3 &p_velocity);
+	Vector3 get_linear_velocity() const;
+
+	void set_angular_velocity(const Vector3 &p_velocity);
+	Vector3 get_angular_velocity() const;
+
+	virtual void set_transform__bullet(const btTransform &p_global_transform);
+	virtual const btTransform &get_transform__bullet() const;
+
+	virtual void on_shapes_changed();
+
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
+	void reload_space_override_modificator();
+
+	/// Kinematic
+	void reload_kinematic_shapes();
+
+private:
+	void _internal_set_mass(real_t p_mass);
+};
+
+#endif

modules/bullet/shape_bullet.cpp

@@ -0,0 +1,435 @@
+/*************************************************************************/
+/*  shape_bullet.cpp                                                     */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "shape_bullet.h"
+#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
+#include "btBulletCollisionCommon.h"
+#include "btRayShape.h"
+#include "bullet_physics_server.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "shape_owner_bullet.h"
+
+ShapeBullet::ShapeBullet() {}
+
+ShapeBullet::~ShapeBullet() {}
+
+btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
+	p_btShape->setUserPointer(const_cast<ShapeBullet *>(this));
+	return p_btShape;
+}
+
+void ShapeBullet::notifyShapeChanged() {
+	for (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
+		static_cast<ShapeOwnerBullet *>(E->key())->on_shape_changed(this);
+	}
+}
+
+void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) {
+	Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
+	if (E) {
+		E->get()++;
+	} else {
+		owners[p_owner] = 1; // add new owner
+	}
+}
+
+void ShapeBullet::remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody) {
+	Map<ShapeOwnerBullet *, int>::Element *E = owners.find(p_owner);
+	ERR_FAIL_COND(!E);
+	E->get()--;
+	if (p_permanentlyFromThisBody || 0 >= E->get()) {
+		owners.erase(E);
+	}
+}
+
+bool ShapeBullet::is_owner(ShapeOwnerBullet *p_owner) const {
+
+	return owners.has(p_owner);
+}
+
+const Map<ShapeOwnerBullet *, int> &ShapeBullet::get_owners() const {
+	return owners;
+}
+
+btEmptyShape *ShapeBullet::create_shape_empty() {
+	return bulletnew(btEmptyShape);
+}
+
+btStaticPlaneShape *ShapeBullet::create_shape_plane(const btVector3 &planeNormal, btScalar planeConstant) {
+	return bulletnew(btStaticPlaneShape(planeNormal, planeConstant));
+}
+
+btSphereShape *ShapeBullet::create_shape_sphere(btScalar radius) {
+	return bulletnew(btSphereShape(radius));
+}
+
+btBoxShape *ShapeBullet::create_shape_box(const btVector3 &boxHalfExtents) {
+	return bulletnew(btBoxShape(boxHalfExtents));
+}
+
+btCapsuleShapeZ *ShapeBullet::create_shape_capsule(btScalar radius, btScalar height) {
+	return bulletnew(btCapsuleShapeZ(radius, height));
+}
+
+btConvexPointCloudShape *ShapeBullet::create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling) {
+	return bulletnew(btConvexPointCloudShape(&p_vertices[0], p_vertices.size(), p_local_scaling));
+}
+
+btScaledBvhTriangleMeshShape *ShapeBullet::create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling) {
+	if (p_mesh_shape) {
+		return bulletnew(btScaledBvhTriangleMeshShape(p_mesh_shape, p_local_scaling));
+	} else {
+		return NULL;
+	}
+}
+
+btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size) {
+	const btScalar ignoredHeightScale(1);
+	const btScalar fieldHeight(500); // Meters
+	const int YAxis = 1; // 0=X, 1=Y, 2=Z
+	const bool flipQuadEdges = false;
+	const void *heightsPtr = p_heights.read().ptr();
+
+	return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, -fieldHeight, fieldHeight, YAxis, PHY_FLOAT, flipQuadEdges));
+}
+
+btRayShape *ShapeBullet::create_shape_ray(real_t p_length) {
+	return bulletnew(btRayShape(p_length));
+}
+
+/* PLANE */
+
+PlaneShapeBullet::PlaneShapeBullet()
+	: ShapeBullet() {}
+
+void PlaneShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+Variant PlaneShapeBullet::get_data() const {
+	return plane;
+}
+
+PhysicsServer::ShapeType PlaneShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_PLANE;
+}
+
+void PlaneShapeBullet::setup(const Plane &p_plane) {
+	plane = p_plane;
+	notifyShapeChanged();
+}
+
+btCollisionShape *PlaneShapeBullet::create_bt_shape() {
+	btVector3 btPlaneNormal;
+	G_TO_B(plane.normal, btPlaneNormal);
+	return prepare(PlaneShapeBullet::create_shape_plane(btPlaneNormal, plane.d));
+}
+
+/* Sphere */
+
+SphereShapeBullet::SphereShapeBullet()
+	: ShapeBullet() {}
+
+void SphereShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+Variant SphereShapeBullet::get_data() const {
+	return radius;
+}
+
+PhysicsServer::ShapeType SphereShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_SPHERE;
+}
+
+void SphereShapeBullet::setup(real_t p_radius) {
+	radius = p_radius;
+	notifyShapeChanged();
+}
+
+btCollisionShape *SphereShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_sphere(radius));
+}
+
+/* Box */
+BoxShapeBullet::BoxShapeBullet()
+	: ShapeBullet() {}
+
+void BoxShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+Variant BoxShapeBullet::get_data() const {
+	Vector3 g_half_extents;
+	B_TO_G(half_extents, g_half_extents);
+	return g_half_extents;
+}
+
+PhysicsServer::ShapeType BoxShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_BOX;
+}
+
+void BoxShapeBullet::setup(const Vector3 &p_half_extents) {
+	G_TO_B(p_half_extents, half_extents);
+	notifyShapeChanged();
+}
+
+btCollisionShape *BoxShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_box(half_extents));
+}
+
+/* Capsule */
+
+CapsuleShapeBullet::CapsuleShapeBullet()
+	: ShapeBullet() {}
+
+void CapsuleShapeBullet::set_data(const Variant &p_data) {
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("radius"));
+	ERR_FAIL_COND(!d.has("height"));
+	setup(d["height"], d["radius"]);
+}
+
+Variant CapsuleShapeBullet::get_data() const {
+	Dictionary d;
+	d["radius"] = radius;
+	d["height"] = height;
+	return d;
+}
+
+PhysicsServer::ShapeType CapsuleShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_CAPSULE;
+}
+
+void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) {
+	radius = p_radius;
+	height = p_height;
+	notifyShapeChanged();
+}
+
+btCollisionShape *CapsuleShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_capsule(radius, height));
+}
+
+/* Convex polygon */
+
+ConvexPolygonShapeBullet::ConvexPolygonShapeBullet()
+	: ShapeBullet() {}
+
+void ConvexPolygonShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+void ConvexPolygonShapeBullet::get_vertices(Vector<Vector3> &out_vertices) {
+	const int n_of_vertices = vertices.size();
+	out_vertices.resize(n_of_vertices);
+	for (int i = n_of_vertices - 1; 0 <= i; --i) {
+		B_TO_G(vertices[i], out_vertices[i]);
+	}
+}
+
+Variant ConvexPolygonShapeBullet::get_data() const {
+	ConvexPolygonShapeBullet *variable_self = const_cast<ConvexPolygonShapeBullet *>(this);
+	Vector<Vector3> out_vertices;
+	variable_self->get_vertices(out_vertices);
+	return out_vertices;
+}
+
+PhysicsServer::ShapeType ConvexPolygonShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_CONVEX_POLYGON;
+}
+
+void ConvexPolygonShapeBullet::setup(const Vector<Vector3> &p_vertices) {
+	// Make a copy of verticies
+	const int n_of_vertices = p_vertices.size();
+	vertices.resize(n_of_vertices);
+	for (int i = n_of_vertices - 1; 0 <= i; --i) {
+		G_TO_B(p_vertices[i], vertices[i]);
+	}
+	notifyShapeChanged();
+}
+
+btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_convex(vertices));
+}
+
+/* Concave polygon */
+
+ConcavePolygonShapeBullet::ConcavePolygonShapeBullet()
+	: ShapeBullet(), meshShape(NULL) {}
+
+ConcavePolygonShapeBullet::~ConcavePolygonShapeBullet() {
+	if (meshShape) {
+		delete meshShape->getMeshInterface();
+		delete meshShape;
+	}
+	faces = PoolVector<Vector3>();
+}
+
+void ConcavePolygonShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+Variant ConcavePolygonShapeBullet::get_data() const {
+	return faces;
+}
+
+PhysicsServer::ShapeType ConcavePolygonShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_CONCAVE_POLYGON;
+}
+
+void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
+	faces = p_faces;
+	if (meshShape) {
+		/// Clear previous created shape
+		delete meshShape->getMeshInterface();
+		bulletdelete(meshShape);
+	}
+	int src_face_count = faces.size();
+	if (0 < src_face_count) {
+
+		btTriangleMesh *shapeInterface = bulletnew(btTriangleMesh);
+
+		// It counts the faces and assert the array contains the correct number of vertices.
+		ERR_FAIL_COND(src_face_count % 3);
+		src_face_count /= 3;
+		PoolVector<Vector3>::Read r = p_faces.read();
+		const Vector3 *facesr = r.ptr();
+
+		btVector3 supVec_0;
+		btVector3 supVec_1;
+		btVector3 supVec_2;
+		for (int i = 0; i < src_face_count; ++i) {
+			G_TO_B(facesr[i * 3], supVec_0);
+			G_TO_B(facesr[i * 3 + 1], supVec_1);
+			G_TO_B(facesr[i * 3 + 2], supVec_2);
+
+			shapeInterface->addTriangle(supVec_0, supVec_1, supVec_2);
+		}
+
+		const bool useQuantizedAabbCompression = true;
+
+		meshShape = bulletnew(btBvhTriangleMeshShape(shapeInterface, useQuantizedAabbCompression));
+	} else {
+		meshShape = NULL;
+		ERR_PRINT("The faces count are 0, the mesh shape cannot be created");
+	}
+	notifyShapeChanged();
+}
+
+btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape() {
+	btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape);
+	if (!cs) {
+		// This is necessary since if 0 faces the creation of concave return NULL
+		cs = ShapeBullet::create_shape_empty();
+	}
+	return prepare(cs);
+}
+
+/* Height map shape */
+
+HeightMapShapeBullet::HeightMapShapeBullet()
+	: ShapeBullet() {}
+
+void HeightMapShapeBullet::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("width"));
+	ERR_FAIL_COND(!d.has("depth"));
+	ERR_FAIL_COND(!d.has("cell_size"));
+	ERR_FAIL_COND(!d.has("heights"));
+
+	int l_width = d["width"];
+	int l_depth = d["depth"];
+	real_t l_cell_size = d["cell_size"];
+	PoolVector<real_t> l_heights = d["heights"];
+
+	ERR_FAIL_COND(l_width <= 0);
+	ERR_FAIL_COND(l_depth <= 0);
+	ERR_FAIL_COND(l_cell_size <= CMP_EPSILON);
+	ERR_FAIL_COND(l_heights.size() != (width * depth));
+	setup(heights, width, depth, cell_size);
+}
+
+Variant HeightMapShapeBullet::get_data() const {
+	ERR_FAIL_V(Variant());
+}
+
+PhysicsServer::ShapeType HeightMapShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_HEIGHTMAP;
+}
+
+void HeightMapShapeBullet::setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size) {
+	{ // Copy
+		const int heights_size = p_heights.size();
+		heights.resize(heights_size);
+		PoolVector<real_t>::Read p_heights_r = p_heights.read();
+		PoolVector<real_t>::Write heights_w = heights.write();
+		for (int i = heights_size - 1; 0 <= i; --i) {
+			heights_w[i] = p_heights_r[i];
+		}
+	}
+	width = p_width;
+	depth = p_depth;
+	cell_size = p_cell_size;
+	notifyShapeChanged();
+}
+
+btCollisionShape *HeightMapShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_height_field(heights, width, depth, cell_size));
+}
+
+/* Ray shape */
+RayShapeBullet::RayShapeBullet()
+	: ShapeBullet(), length(1) {}
+
+void RayShapeBullet::set_data(const Variant &p_data) {
+	setup(p_data);
+}
+
+Variant RayShapeBullet::get_data() const {
+	return length;
+}
+
+PhysicsServer::ShapeType RayShapeBullet::get_type() const {
+	return PhysicsServer::SHAPE_RAY;
+}
+
+void RayShapeBullet::setup(real_t p_length) {
+	length = p_length;
+	notifyShapeChanged();
+}
+
+btCollisionShape *RayShapeBullet::create_bt_shape() {
+	return prepare(ShapeBullet::create_shape_ray(length));
+}

modules/bullet/shape_bullet.h

@@ -0,0 +1,225 @@
+/*************************************************************************/
+/*  shape_bullet.h                                                       */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef SHAPE_BULLET_H
+#define SHAPE_BULLET_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "core/variant.h"
+#include "geometry.h"
+#include "rid_bullet.h"
+#include "servers/physics_server.h"
+
+class ShapeBullet;
+class btCollisionShape;
+class ShapeOwnerBullet;
+class btBvhTriangleMeshShape;
+
+class ShapeBullet : public RIDBullet {
+
+	Map<ShapeOwnerBullet *, int> owners;
+
+protected:
+	/// return self
+	btCollisionShape *prepare(btCollisionShape *p_btShape) const;
+	void notifyShapeChanged();
+
+public:
+	ShapeBullet();
+	virtual ~ShapeBullet();
+
+	virtual btCollisionShape *create_bt_shape() = 0;
+
+	void add_owner(ShapeOwnerBullet *p_owner);
+	void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false);
+	bool is_owner(ShapeOwnerBullet *p_owner) const;
+	const Map<ShapeOwnerBullet *, int> &get_owners() const;
+
+	/// Setup the shape
+	virtual void set_data(const Variant &p_data) = 0;
+	virtual Variant get_data() const = 0;
+
+	virtual PhysicsServer::ShapeType get_type() const = 0;
+
+public:
+	static class btEmptyShape *create_shape_empty();
+	static class btStaticPlaneShape *create_shape_plane(const btVector3 &planeNormal, btScalar planeConstant);
+	static class btSphereShape *create_shape_sphere(btScalar radius);
+	static class btBoxShape *create_shape_box(const btVector3 &boxHalfExtents);
+	static class btCapsuleShapeZ *create_shape_capsule(btScalar radius, btScalar height);
+	/// IMPORTANT: Remember to delete the shape interface by calling: delete my_shape->getMeshInterface();
+	static class btConvexPointCloudShape *create_shape_convex(btAlignedObjectArray<btVector3> &p_vertices, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
+	static class btScaledBvhTriangleMeshShape *create_shape_concave(btBvhTriangleMeshShape *p_mesh_shape, const btVector3 &p_local_scaling = btVector3(1, 1, 1));
+	static class btHeightfieldTerrainShape *create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
+	static class btRayShape *create_shape_ray(real_t p_length);
+};
+
+class PlaneShapeBullet : public ShapeBullet {
+
+	Plane plane;
+
+public:
+	PlaneShapeBullet();
+
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(const Plane &p_plane);
+};
+
+class SphereShapeBullet : public ShapeBullet {
+
+	real_t radius;
+
+public:
+	SphereShapeBullet();
+
+	_FORCE_INLINE_ real_t get_radius() { return radius; }
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(real_t p_radius);
+};
+
+class BoxShapeBullet : public ShapeBullet {
+
+	btVector3 half_extents;
+
+public:
+	BoxShapeBullet();
+
+	_FORCE_INLINE_ const btVector3 &get_half_extents() { return half_extents; }
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(const Vector3 &p_half_extents);
+};
+
+class CapsuleShapeBullet : public ShapeBullet {
+
+	real_t height;
+	real_t radius;
+
+public:
+	CapsuleShapeBullet();
+
+	_FORCE_INLINE_ real_t get_height() { return height; }
+	_FORCE_INLINE_ real_t get_radius() { return radius; }
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(real_t p_height, real_t p_radius);
+};
+
+class ConvexPolygonShapeBullet : public ShapeBullet {
+
+public:
+	btAlignedObjectArray<btVector3> vertices;
+
+	ConvexPolygonShapeBullet();
+
+	virtual void set_data(const Variant &p_data);
+	void get_vertices(Vector<Vector3> &out_vertices);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(const Vector<Vector3> &p_vertices);
+};
+
+class ConcavePolygonShapeBullet : public ShapeBullet {
+	class btBvhTriangleMeshShape *meshShape;
+
+public:
+	PoolVector<Vector3> faces;
+
+	ConcavePolygonShapeBullet();
+	virtual ~ConcavePolygonShapeBullet();
+
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(PoolVector<Vector3> p_faces);
+};
+
+class HeightMapShapeBullet : public ShapeBullet {
+
+public:
+	PoolVector<real_t> heights;
+	int width;
+	int depth;
+	real_t cell_size;
+
+	HeightMapShapeBullet();
+
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
+};
+
+class RayShapeBullet : public ShapeBullet {
+
+public:
+	real_t length;
+
+	RayShapeBullet();
+
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+	virtual PhysicsServer::ShapeType get_type() const;
+	virtual btCollisionShape *create_bt_shape();
+
+private:
+	void setup(real_t p_length);
+};
+#endif

modules/bullet/shape_owner_bullet.cpp

@@ -0,0 +1,32 @@
+/*************************************************************************/
+/*  shape_owner_bullet.cpp                                               */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "shape_owner_bullet.h"

modules/bullet/shape_owner_bullet.h

@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  shape_owner_bullet.h                                                 */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef SHAPE_OWNER_BULLET_H
+#define SHAPE_OWNER_BULLET_H
+
+#include "rid_bullet.h"
+
+class ShapeBullet;
+class btCollisionShape;
+class CollisionObjectBullet;
+
+/// Each clas that want to use Shapes must inherit this class
+/// E.G. BodyShape is a child of this
+class ShapeOwnerBullet {
+public:
+	/// This is used to set new shape or replace existing
+	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
+	virtual void on_shape_changed(const ShapeBullet *const p_shape) = 0;
+	virtual void on_shapes_changed() = 0;
+	virtual void remove_shape(class ShapeBullet *p_shape) = 0;
+	virtual ~ShapeOwnerBullet() {}
+};
+#endif

modules/bullet/slider_joint_bullet.cpp

@@ -0,0 +1,385 @@
+/*************************************************************************/
+/*  slider_joint_bullet.cpp                                              */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "slider_joint_bullet.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "rigid_body_bullet.h"
+
+SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB)
+	: JointBullet() {
+	btTransform btFrameA;
+	G_TO_B(frameInA, btFrameA);
+	if (rbB) {
+		btTransform btFrameB;
+		G_TO_B(frameInB, btFrameB);
+		sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
+
+	} else {
+		sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), btFrameA, true));
+	}
+	setup(sliderConstraint);
+}
+
+const RigidBodyBullet *SliderJointBullet::getRigidBodyA() const {
+	return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyA().getUserPointer());
+}
+
+const RigidBodyBullet *SliderJointBullet::getRigidBodyB() const {
+	return static_cast<RigidBodyBullet *>(sliderConstraint->getRigidBodyB().getUserPointer());
+}
+
+const Transform SliderJointBullet::getCalculatedTransformA() const {
+	btTransform btTransform = sliderConstraint->getCalculatedTransformA();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+const Transform SliderJointBullet::getCalculatedTransformB() const {
+	btTransform btTransform = sliderConstraint->getCalculatedTransformB();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+const Transform SliderJointBullet::getFrameOffsetA() const {
+	btTransform btTransform = sliderConstraint->getFrameOffsetA();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+const Transform SliderJointBullet::getFrameOffsetB() const {
+	btTransform btTransform = sliderConstraint->getFrameOffsetB();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+Transform SliderJointBullet::getFrameOffsetA() {
+	btTransform btTransform = sliderConstraint->getFrameOffsetA();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+Transform SliderJointBullet::getFrameOffsetB() {
+	btTransform btTransform = sliderConstraint->getFrameOffsetB();
+	Transform gTrans;
+	B_TO_G(btTransform, gTrans);
+	return gTrans;
+}
+
+real_t SliderJointBullet::getLowerLinLimit() const {
+	return sliderConstraint->getLowerLinLimit();
+}
+
+void SliderJointBullet::setLowerLinLimit(real_t lowerLimit) {
+	sliderConstraint->setLowerLinLimit(lowerLimit);
+}
+real_t SliderJointBullet::getUpperLinLimit() const {
+	return sliderConstraint->getUpperLinLimit();
+}
+
+void SliderJointBullet::setUpperLinLimit(real_t upperLimit) {
+	sliderConstraint->setUpperLinLimit(upperLimit);
+}
+
+real_t SliderJointBullet::getLowerAngLimit() const {
+	return sliderConstraint->getLowerAngLimit();
+}
+
+void SliderJointBullet::setLowerAngLimit(real_t lowerLimit) {
+	sliderConstraint->setLowerAngLimit(lowerLimit);
+}
+
+real_t SliderJointBullet::getUpperAngLimit() const {
+	return sliderConstraint->getUpperAngLimit();
+}
+
+void SliderJointBullet::setUpperAngLimit(real_t upperLimit) {
+	sliderConstraint->setUpperAngLimit(upperLimit);
+}
+
+real_t SliderJointBullet::getSoftnessDirLin() const {
+	return sliderConstraint->getSoftnessDirLin();
+}
+
+real_t SliderJointBullet::getRestitutionDirLin() const {
+	return sliderConstraint->getRestitutionDirLin();
+}
+
+real_t SliderJointBullet::getDampingDirLin() const {
+	return sliderConstraint->getDampingDirLin();
+}
+
+real_t SliderJointBullet::getSoftnessDirAng() const {
+	return sliderConstraint->getSoftnessDirAng();
+}
+
+real_t SliderJointBullet::getRestitutionDirAng() const {
+	return sliderConstraint->getRestitutionDirAng();
+}
+
+real_t SliderJointBullet::getDampingDirAng() const {
+	return sliderConstraint->getDampingDirAng();
+}
+
+real_t SliderJointBullet::getSoftnessLimLin() const {
+	return sliderConstraint->getSoftnessLimLin();
+}
+
+real_t SliderJointBullet::getRestitutionLimLin() const {
+	return sliderConstraint->getRestitutionLimLin();
+}
+
+real_t SliderJointBullet::getDampingLimLin() const {
+	return sliderConstraint->getDampingLimLin();
+}
+
+real_t SliderJointBullet::getSoftnessLimAng() const {
+	return sliderConstraint->getSoftnessLimAng();
+}
+
+real_t SliderJointBullet::getRestitutionLimAng() const {
+	return sliderConstraint->getRestitutionLimAng();
+}
+
+real_t SliderJointBullet::getDampingLimAng() const {
+	return sliderConstraint->getDampingLimAng();
+}
+
+real_t SliderJointBullet::getSoftnessOrthoLin() const {
+	return sliderConstraint->getSoftnessOrthoLin();
+}
+
+real_t SliderJointBullet::getRestitutionOrthoLin() const {
+	return sliderConstraint->getRestitutionOrthoLin();
+}
+
+real_t SliderJointBullet::getDampingOrthoLin() const {
+	return sliderConstraint->getDampingOrthoLin();
+}
+
+real_t SliderJointBullet::getSoftnessOrthoAng() const {
+	return sliderConstraint->getSoftnessOrthoAng();
+}
+
+real_t SliderJointBullet::getRestitutionOrthoAng() const {
+	return sliderConstraint->getRestitutionOrthoAng();
+}
+
+real_t SliderJointBullet::getDampingOrthoAng() const {
+	return sliderConstraint->getDampingOrthoAng();
+}
+
+void SliderJointBullet::setSoftnessDirLin(real_t softnessDirLin) {
+	sliderConstraint->setSoftnessDirLin(softnessDirLin);
+}
+
+void SliderJointBullet::setRestitutionDirLin(real_t restitutionDirLin) {
+	sliderConstraint->setRestitutionDirLin(restitutionDirLin);
+}
+
+void SliderJointBullet::setDampingDirLin(real_t dampingDirLin) {
+	sliderConstraint->setDampingDirLin(dampingDirLin);
+}
+
+void SliderJointBullet::setSoftnessDirAng(real_t softnessDirAng) {
+	sliderConstraint->setSoftnessDirAng(softnessDirAng);
+}
+
+void SliderJointBullet::setRestitutionDirAng(real_t restitutionDirAng) {
+	sliderConstraint->setRestitutionDirAng(restitutionDirAng);
+}
+
+void SliderJointBullet::setDampingDirAng(real_t dampingDirAng) {
+	sliderConstraint->setDampingDirAng(dampingDirAng);
+}
+
+void SliderJointBullet::setSoftnessLimLin(real_t softnessLimLin) {
+	sliderConstraint->setSoftnessLimLin(softnessLimLin);
+}
+
+void SliderJointBullet::setRestitutionLimLin(real_t restitutionLimLin) {
+	sliderConstraint->setRestitutionLimLin(restitutionLimLin);
+}
+
+void SliderJointBullet::setDampingLimLin(real_t dampingLimLin) {
+	sliderConstraint->setDampingLimLin(dampingLimLin);
+}
+
+void SliderJointBullet::setSoftnessLimAng(real_t softnessLimAng) {
+	sliderConstraint->setSoftnessLimAng(softnessLimAng);
+}
+
+void SliderJointBullet::setRestitutionLimAng(real_t restitutionLimAng) {
+	sliderConstraint->setRestitutionLimAng(restitutionLimAng);
+}
+
+void SliderJointBullet::setDampingLimAng(real_t dampingLimAng) {
+	sliderConstraint->setDampingLimAng(dampingLimAng);
+}
+
+void SliderJointBullet::setSoftnessOrthoLin(real_t softnessOrthoLin) {
+	sliderConstraint->setSoftnessOrthoLin(softnessOrthoLin);
+}
+
+void SliderJointBullet::setRestitutionOrthoLin(real_t restitutionOrthoLin) {
+	sliderConstraint->setRestitutionOrthoLin(restitutionOrthoLin);
+}
+
+void SliderJointBullet::setDampingOrthoLin(real_t dampingOrthoLin) {
+	sliderConstraint->setDampingOrthoLin(dampingOrthoLin);
+}
+
+void SliderJointBullet::setSoftnessOrthoAng(real_t softnessOrthoAng) {
+	sliderConstraint->setSoftnessOrthoAng(softnessOrthoAng);
+}
+
+void SliderJointBullet::setRestitutionOrthoAng(real_t restitutionOrthoAng) {
+	sliderConstraint->setRestitutionOrthoAng(restitutionOrthoAng);
+}
+
+void SliderJointBullet::setDampingOrthoAng(real_t dampingOrthoAng) {
+	sliderConstraint->setDampingOrthoAng(dampingOrthoAng);
+}
+
+void SliderJointBullet::setPoweredLinMotor(bool onOff) {
+	sliderConstraint->setPoweredLinMotor(onOff);
+}
+
+bool SliderJointBullet::getPoweredLinMotor() {
+	return sliderConstraint->getPoweredLinMotor();
+}
+
+void SliderJointBullet::setTargetLinMotorVelocity(real_t targetLinMotorVelocity) {
+	sliderConstraint->setTargetLinMotorVelocity(targetLinMotorVelocity);
+}
+
+real_t SliderJointBullet::getTargetLinMotorVelocity() {
+	return sliderConstraint->getTargetLinMotorVelocity();
+}
+
+void SliderJointBullet::setMaxLinMotorForce(real_t maxLinMotorForce) {
+	sliderConstraint->setMaxLinMotorForce(maxLinMotorForce);
+}
+
+real_t SliderJointBullet::getMaxLinMotorForce() {
+	return sliderConstraint->getMaxLinMotorForce();
+}
+
+void SliderJointBullet::setPoweredAngMotor(bool onOff) {
+	sliderConstraint->setPoweredAngMotor(onOff);
+}
+
+bool SliderJointBullet::getPoweredAngMotor() {
+	return sliderConstraint->getPoweredAngMotor();
+}
+
+void SliderJointBullet::setTargetAngMotorVelocity(real_t targetAngMotorVelocity) {
+	sliderConstraint->setTargetAngMotorVelocity(targetAngMotorVelocity);
+}
+
+real_t SliderJointBullet::getTargetAngMotorVelocity() {
+	return sliderConstraint->getTargetAngMotorVelocity();
+}
+
+void SliderJointBullet::setMaxAngMotorForce(real_t maxAngMotorForce) {
+	sliderConstraint->setMaxAngMotorForce(maxAngMotorForce);
+}
+
+real_t SliderJointBullet::getMaxAngMotorForce() {
+	return sliderConstraint->getMaxAngMotorForce();
+}
+
+real_t SliderJointBullet::getLinearPos() {
+	return sliderConstraint->getLinearPos();
+	;
+}
+
+void SliderJointBullet::set_param(PhysicsServer::SliderJointParam p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: setUpperLinLimit(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: setLowerLinLimit(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: setSoftnessLimLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: setRestitutionLimLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: setDampingLimLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: setSoftnessDirLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: setRestitutionDirLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: setDampingDirLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: setSoftnessOrthoLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: setRestitutionOrthoLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: setDampingOrthoLin(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: setUpperAngLimit(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: setLowerAngLimit(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: setSoftnessLimAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: setRestitutionLimAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: setDampingLimAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: setSoftnessDirAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: setRestitutionDirAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: setDampingDirAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: setSoftnessOrthoAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: setRestitutionOrthoAng(p_value); break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: setDampingOrthoAng(p_value); break;
+	}
+}
+
+real_t SliderJointBullet::get_param(PhysicsServer::SliderJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return getUpperLinLimit();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return getLowerLinLimit();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return getSoftnessLimLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return getRestitutionLimLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return getDampingLimLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return getSoftnessDirLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return getRestitutionDirLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return getDampingDirLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoLin();
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return getDampingOrthoLin();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return getUpperAngLimit();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return getLowerAngLimit();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return getSoftnessLimAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return getRestitutionLimAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return getDampingLimAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return getSoftnessDirAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return getRestitutionDirAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return getDampingDirAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return getSoftnessOrthoAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return getRestitutionOrthoAng();
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return getDampingOrthoAng();
+		default:
+			return 0;
+	}
+}

modules/bullet/slider_joint_bullet.h

@@ -0,0 +1,118 @@
+/*************************************************************************/
+/*  slider_joint_bullet.h                                                */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef SLIDER_JOINT_BULLET_H
+#define SLIDER_JOINT_BULLET_H
+
+#include "joint_bullet.h"
+
+class RigidBodyBullet;
+
+class SliderJointBullet : public JointBullet {
+	class btSliderConstraint *sliderConstraint;
+
+public:
+	/// Reference frame is A
+	SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB);
+
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
+
+	const RigidBodyBullet *getRigidBodyA() const;
+	const RigidBodyBullet *getRigidBodyB() const;
+	const Transform getCalculatedTransformA() const;
+	const Transform getCalculatedTransformB() const;
+	const Transform getFrameOffsetA() const;
+	const Transform getFrameOffsetB() const;
+	Transform getFrameOffsetA();
+	Transform getFrameOffsetB();
+	real_t getLowerLinLimit() const;
+	void setLowerLinLimit(real_t lowerLimit);
+	real_t getUpperLinLimit() const;
+	void setUpperLinLimit(real_t upperLimit);
+	real_t getLowerAngLimit() const;
+	void setLowerAngLimit(real_t lowerLimit);
+	real_t getUpperAngLimit() const;
+	void setUpperAngLimit(real_t upperLimit);
+
+	real_t getSoftnessDirLin() const;
+	real_t getRestitutionDirLin() const;
+	real_t getDampingDirLin() const;
+	real_t getSoftnessDirAng() const;
+	real_t getRestitutionDirAng() const;
+	real_t getDampingDirAng() const;
+	real_t getSoftnessLimLin() const;
+	real_t getRestitutionLimLin() const;
+	real_t getDampingLimLin() const;
+	real_t getSoftnessLimAng() const;
+	real_t getRestitutionLimAng() const;
+	real_t getDampingLimAng() const;
+	real_t getSoftnessOrthoLin() const;
+	real_t getRestitutionOrthoLin() const;
+	real_t getDampingOrthoLin() const;
+	real_t getSoftnessOrthoAng() const;
+	real_t getRestitutionOrthoAng() const;
+	real_t getDampingOrthoAng() const;
+	void setSoftnessDirLin(real_t softnessDirLin);
+	void setRestitutionDirLin(real_t restitutionDirLin);
+	void setDampingDirLin(real_t dampingDirLin);
+	void setSoftnessDirAng(real_t softnessDirAng);
+	void setRestitutionDirAng(real_t restitutionDirAng);
+	void setDampingDirAng(real_t dampingDirAng);
+	void setSoftnessLimLin(real_t softnessLimLin);
+	void setRestitutionLimLin(real_t restitutionLimLin);
+	void setDampingLimLin(real_t dampingLimLin);
+	void setSoftnessLimAng(real_t softnessLimAng);
+	void setRestitutionLimAng(real_t restitutionLimAng);
+	void setDampingLimAng(real_t dampingLimAng);
+	void setSoftnessOrthoLin(real_t softnessOrthoLin);
+	void setRestitutionOrthoLin(real_t restitutionOrthoLin);
+	void setDampingOrthoLin(real_t dampingOrthoLin);
+	void setSoftnessOrthoAng(real_t softnessOrthoAng);
+	void setRestitutionOrthoAng(real_t restitutionOrthoAng);
+	void setDampingOrthoAng(real_t dampingOrthoAng);
+	void setPoweredLinMotor(bool onOff);
+	bool getPoweredLinMotor();
+	void setTargetLinMotorVelocity(real_t targetLinMotorVelocity);
+	real_t getTargetLinMotorVelocity();
+	void setMaxLinMotorForce(real_t maxLinMotorForce);
+	real_t getMaxLinMotorForce();
+	void setPoweredAngMotor(bool onOff);
+	bool getPoweredAngMotor();
+	void setTargetAngMotorVelocity(real_t targetAngMotorVelocity);
+	real_t getTargetAngMotorVelocity();
+	void setMaxAngMotorForce(real_t maxAngMotorForce);
+	real_t getMaxAngMotorForce();
+	real_t getLinearPos();
+
+	void set_param(PhysicsServer::SliderJointParam p_param, real_t p_value);
+	real_t get_param(PhysicsServer::SliderJointParam p_param) const;
+};
+#endif

modules/bullet/soft_body_bullet.cpp

@@ -0,0 +1,303 @@
+/*************************************************************************/
+/*  soft_body_bullet.cpp                                                 */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "soft_body_bullet.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "space_bullet.h"
+
+#include "scene/3d/immediate_geometry.h"
+
+SoftBodyBullet::SoftBodyBullet()
+	: CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY), mass(1), simulation_precision(5), stiffness(0.5f), pressure_coefficient(50), damping_coefficient(0.005), drag_coefficient(0.005), bt_soft_body(NULL), soft_shape_type(SOFT_SHAPETYPE_NONE), isScratched(false), soft_body_shape_data(NULL) {
+
+	test_geometry = memnew(ImmediateGeometry);
+
+	red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
+	red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
+	red_mat->set_line_width(20.0);
+	red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
+	red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
+	red_mat->set_albedo(Color(1, 0, 0, 1));
+	test_geometry->set_material_override(red_mat);
+
+	test_is_in_scene = false;
+}
+
+SoftBodyBullet::~SoftBodyBullet() {
+	bulletdelete(soft_body_shape_data);
+}
+
+void SoftBodyBullet::reload_body() {
+	if (space) {
+		space->remove_soft_body(this);
+		space->add_soft_body(this);
+	}
+}
+
+void SoftBodyBullet::set_space(SpaceBullet *p_space) {
+	if (space) {
+		isScratched = false;
+
+		// Remove this object from the physics world
+		space->remove_soft_body(this);
+	}
+
+	space = p_space;
+
+	if (space) {
+		space->add_soft_body(this);
+	}
+
+	reload_soft_body();
+}
+
+void SoftBodyBullet::dispatch_callbacks() {
+	if (!bt_soft_body) {
+		return;
+	}
+
+	if (!test_is_in_scene) {
+		test_is_in_scene = true;
+		SceneTree::get_singleton()->get_current_scene()->add_child(test_geometry);
+	}
+
+	test_geometry->clear();
+	test_geometry->begin(Mesh::PRIMITIVE_LINES, NULL);
+	bool first = true;
+	Vector3 pos;
+	for (int i = 0; i < bt_soft_body->m_nodes.size(); ++i) {
+		const btSoftBody::Node &n = bt_soft_body->m_nodes[i];
+		B_TO_G(n.m_x, pos);
+		test_geometry->add_vertex(pos);
+		if (!first) {
+			test_geometry->add_vertex(pos);
+		} else {
+			first = false;
+		}
+	}
+	test_geometry->end();
+}
+
+void SoftBodyBullet::on_collision_filters_change() {
+}
+
+void SoftBodyBullet::on_collision_checker_start() {
+}
+
+void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {
+}
+
+void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {
+}
+
+void SoftBodyBullet::set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num) {
+
+	TrimeshSoftShapeData *shape_data = bulletnew(TrimeshSoftShapeData);
+	shape_data->m_triangles_indices = p_indices;
+	shape_data->m_vertices = p_vertices;
+	shape_data->m_triangles_num = p_triangles_num;
+
+	set_body_shape_data(shape_data, SOFT_SHAPE_TYPE_TRIMESH);
+	reload_soft_body();
+}
+
+void SoftBodyBullet::set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type) {
+	bulletdelete(soft_body_shape_data);
+	soft_body_shape_data = p_soft_shape_data;
+	soft_shape_type = p_type;
+}
+
+void SoftBodyBullet::set_transform(const Transform &p_transform) {
+	transform = p_transform;
+	if (bt_soft_body) {
+		// TODO the softbody set new transform considering the current transform as center of world
+		// like if it's local transform, so I must fix this by setting nwe transform considering the old
+		btTransform bt_trans;
+		G_TO_B(transform, bt_trans);
+		//bt_soft_body->transform(bt_trans);
+	}
+}
+
+const Transform &SoftBodyBullet::get_transform() const {
+	return transform;
+}
+
+void SoftBodyBullet::get_first_node_origin(btVector3 &p_out_origin) const {
+	if (bt_soft_body && bt_soft_body->m_nodes.size()) {
+		p_out_origin = bt_soft_body->m_nodes[0].m_x;
+	} else {
+		p_out_origin.setZero();
+	}
+}
+
+void SoftBodyBullet::set_activation_state(bool p_active) {
+	if (p_active) {
+		bt_soft_body->setActivationState(ACTIVE_TAG);
+	} else {
+		bt_soft_body->setActivationState(WANTS_DEACTIVATION);
+	}
+}
+
+void SoftBodyBullet::set_mass(real_t p_val) {
+	if (0 >= p_val) {
+		p_val = 1;
+	}
+	mass = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->setTotalMass(mass);
+	}
+}
+
+void SoftBodyBullet::set_stiffness(real_t p_val) {
+	stiffness = p_val;
+	if (bt_soft_body) {
+		mat0->m_kAST = stiffness;
+		mat0->m_kLST = stiffness;
+		mat0->m_kVST = stiffness;
+	}
+}
+
+void SoftBodyBullet::set_simulation_precision(int p_val) {
+	simulation_precision = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->m_cfg.piterations = simulation_precision;
+	}
+}
+
+void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
+	pressure_coefficient = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->m_cfg.kPR = pressure_coefficient;
+	}
+}
+
+void SoftBodyBullet::set_damping_coefficient(real_t p_val) {
+	damping_coefficient = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->m_cfg.kDP = damping_coefficient;
+	}
+}
+
+void SoftBodyBullet::set_drag_coefficient(real_t p_val) {
+	drag_coefficient = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->m_cfg.kDG = drag_coefficient;
+	}
+}
+
+void SoftBodyBullet::reload_soft_body() {
+
+	destroy_soft_body();
+	create_soft_body();
+
+	if (bt_soft_body) {
+
+		// TODO the softbody set new transform considering the current transform as center of world
+		// like if it's local transform, so I must fix this by setting nwe transform considering the old
+		btTransform bt_trans;
+		G_TO_B(transform, bt_trans);
+		bt_soft_body->transform(bt_trans);
+
+		bt_soft_body->generateBendingConstraints(2, mat0);
+		mat0->m_kAST = stiffness;
+		mat0->m_kLST = stiffness;
+		mat0->m_kVST = stiffness;
+
+		bt_soft_body->m_cfg.piterations = simulation_precision;
+		bt_soft_body->m_cfg.kDP = damping_coefficient;
+		bt_soft_body->m_cfg.kDG = drag_coefficient;
+		bt_soft_body->m_cfg.kPR = pressure_coefficient;
+		bt_soft_body->setTotalMass(mass);
+	}
+	if (space) {
+		// TODO remove this please
+		space->add_soft_body(this);
+	}
+}
+
+void SoftBodyBullet::create_soft_body() {
+	if (!space || !soft_body_shape_data) {
+		return;
+	}
+	ERR_FAIL_COND(!space->is_using_soft_world());
+	switch (soft_shape_type) {
+		case SOFT_SHAPE_TYPE_TRIMESH: {
+			TrimeshSoftShapeData *trimesh_data = static_cast<TrimeshSoftShapeData *>(soft_body_shape_data);
+
+			Vector<int> indices;
+			Vector<btScalar> vertices;
+
+			int i;
+			const int indices_size = trimesh_data->m_triangles_indices.size();
+			const int vertices_size = trimesh_data->m_vertices.size();
+			indices.resize(indices_size);
+			vertices.resize(vertices_size * 3);
+
+			PoolVector<int>::Read i_r = trimesh_data->m_triangles_indices.read();
+			for (i = 0; i < indices_size; ++i) {
+				indices[i] = i_r[i];
+			}
+			i_r = PoolVector<int>::Read();
+
+			PoolVector<Vector3>::Read f_r = trimesh_data->m_vertices.read();
+			for (int j = i = 0; i < vertices_size; ++i, j += 3) {
+				vertices[j + 0] = f_r[i][0];
+				vertices[j + 1] = f_r[i][1];
+				vertices[j + 2] = f_r[i][2];
+			}
+			f_r = PoolVector<Vector3>::Read();
+
+			bt_soft_body = btSoftBodyHelpers::CreateFromTriMesh(*space->get_soft_body_world_info(), vertices.ptr(), indices.ptr(), trimesh_data->m_triangles_num);
+		} break;
+		default:
+			ERR_PRINT("Shape type not supported");
+			return;
+	}
+
+	setupBulletCollisionObject(bt_soft_body);
+	bt_soft_body->getCollisionShape()->setMargin(0.001f);
+	bt_soft_body->setCollisionFlags(bt_soft_body->getCollisionFlags() & (~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)));
+	mat0 = bt_soft_body->appendMaterial();
+}
+
+void SoftBodyBullet::destroy_soft_body() {
+	if (space) {
+		/// This step is required to assert that the body is not into the world during deletion
+		/// This step is required since to change the body shape the body must be re-created.
+		/// Here is handled the case when the body is assigned into a world and the body
+		/// shape is changed.
+		space->remove_soft_body(this);
+	}
+	destroyBulletCollisionObject();
+	bt_soft_body = NULL;
+}

modules/bullet/soft_body_bullet.h

@@ -0,0 +1,136 @@
+/*************************************************************************/
+/*  soft_body_bullet.h                                                   */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef SOFT_BODY_BULLET_H
+#define SOFT_BODY_BULLET_H
+
+#ifdef None
+/// This is required to remove the macro None defined by x11 compiler because this word "None" is used internally by Bullet
+#undef None
+#define x11_None 0L
+#endif
+
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "collision_object_bullet.h"
+
+#ifdef x11_None
+/// This is required to re add the macro None defined by x11 compiler
+#undef x11_None
+#define None 0L
+#endif
+
+#include "scene/resources/material.h" // TODO remove thsi please
+
+struct SoftShapeData {};
+struct TrimeshSoftShapeData : public SoftShapeData {
+	PoolVector<int> m_triangles_indices;
+	PoolVector<Vector3> m_vertices;
+	int m_triangles_num;
+};
+
+class SoftBodyBullet : public CollisionObjectBullet {
+public:
+	enum SoftShapeType {
+		SOFT_SHAPETYPE_NONE = 0,
+		SOFT_SHAPE_TYPE_TRIMESH
+	};
+
+private:
+	btSoftBody *bt_soft_body;
+	btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody
+	SoftShapeType soft_shape_type;
+	bool isScratched;
+
+	SoftShapeData *soft_body_shape_data;
+
+	Transform transform;
+	int simulation_precision;
+	real_t mass;
+	real_t stiffness; // [0,1]
+	real_t pressure_coefficient; // [-inf,+inf]
+	real_t damping_coefficient; // [0,1]
+	real_t drag_coefficient; // [0,1]
+
+	class ImmediateGeometry *test_geometry; // TODO remove this please
+	Ref<SpatialMaterial> red_mat; // TODO remove this please
+	bool test_is_in_scene; // TODO remove this please
+
+public:
+	SoftBodyBullet();
+	~SoftBodyBullet();
+
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
+
+	virtual void dispatch_callbacks();
+	virtual void on_collision_filters_change();
+	virtual void on_collision_checker_start();
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
+
+	_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
+
+	void set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
+	void set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type);
+
+	void set_transform(const Transform &p_transform);
+	/// This function doesn't return the exact COM transform.
+	/// It returns the origin only of first node (vertice) of current soft body
+	/// ---
+	/// The soft body doesn't have a fixed center of mass, but is a group of nodes (vertices)
+	/// that each has its own position in the world.
+	/// For this reason return the correct COM is not so simple and must be calculate
+	/// Check this to improve this function http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=8803
+	const Transform &get_transform() const;
+	void get_first_node_origin(btVector3 &p_out_origin) const;
+
+	void set_activation_state(bool p_active);
+
+	void set_mass(real_t p_val);
+	_FORCE_INLINE_ real_t get_mass() const { return mass; }
+	void set_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_stiffness() const { return stiffness; }
+	void set_simulation_precision(int p_val);
+	_FORCE_INLINE_ int get_simulation_precision() const { return simulation_precision; }
+	void set_pressure_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
+	void set_damping_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
+	void set_drag_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
+
+private:
+	void reload_soft_body();
+	void create_soft_body();
+	void destroy_soft_body();
+};
+
+#endif // SOFT_BODY_BULLET_H

modules/bullet/space_bullet.cpp

@@ -0,0 +1,1163 @@
+/*************************************************************************/
+/*  space_bullet.cpp                                                     */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "space_bullet.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btGhostObject.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
+#include "btBulletDynamicsCommon.h"
+#include "bullet_physics_server.h"
+#include "bullet_types_converter.h"
+#include "bullet_utilities.h"
+#include "constraint_bullet.h"
+#include "godot_collision_configuration.h"
+#include "godot_collision_dispatcher.h"
+#include "rigid_body_bullet.h"
+#include "servers/physics_server.h"
+#include "soft_body_bullet.h"
+#include "ustring.h"
+#include <assert.h>
+
+// test only
+//#include "scene/3d/immediate_geometry.h"
+
+BulletPhysicsDirectSpaceState::BulletPhysicsDirectSpaceState(SpaceBullet *p_space)
+	: PhysicsDirectSpaceState(), space(p_space) {}
+
+int BulletPhysicsDirectSpaceState::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+
+	if (p_result_max <= 0)
+		return 0;
+
+	btVector3 bt_point;
+	G_TO_B(p_point, bt_point);
+
+	btSphereShape sphere_point(0.f);
+	btCollisionObject collision_object_point;
+	collision_object_point.setCollisionShape(&sphere_point);
+	collision_object_point.setWorldTransform(btTransform(btQuaternion::getIdentity(), bt_point));
+
+	// Setup query
+	GodotAllContactResultCallback btResult(&collision_object_point, r_results, p_result_max, &p_exclude);
+	btResult.m_collisionFilterGroup = p_collision_layer;
+	btResult.m_collisionFilterMask = p_object_type_mask;
+	space->dynamicsWorld->contactTest(&collision_object_point, btResult);
+
+	// The results is already populated by GodotAllConvexResultCallback
+	return btResult.m_count;
+}
+
+bool BulletPhysicsDirectSpaceState::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) {
+
+	btVector3 btVec_from;
+	btVector3 btVec_to;
+
+	G_TO_B(p_from, btVec_from);
+	G_TO_B(p_to, btVec_to);
+
+	// setup query
+	GodotClosestRayResultCallback btResult(btVec_from, btVec_to, &p_exclude);
+	btResult.m_collisionFilterGroup = p_collision_layer;
+	btResult.m_collisionFilterMask = p_object_type_mask;
+	btResult.m_pickRay = p_pick_ray;
+
+	space->dynamicsWorld->rayTest(btVec_from, btVec_to, btResult);
+	if (btResult.hasHit()) {
+		B_TO_G(btResult.m_hitPointWorld, r_result.position);
+		B_TO_G(btResult.m_hitNormalWorld.normalize(), r_result.normal);
+		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btResult.m_collisionObject->getUserPointer());
+		if (gObj) {
+			r_result.shape = 0;
+			r_result.rid = gObj->get_self();
+			r_result.collider_id = gObj->get_instance_id();
+			r_result.collider = 0 == r_result.collider_id ? NULL : ObjectDB::get_instance(r_result.collider_id);
+		} else {
+			WARN_PRINTS("The raycast performed has hit a collision object that is not part of Godot scene, please check it.");
+		}
+		return true;
+	} else {
+		return false;
+	}
+}
+
+int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *p_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+	if (p_result_max <= 0)
+		return 0;
+
+	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
+
+	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
+	if (!btConvex) {
+		bulletdelete(btConvex);
+		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
+		return 0;
+	}
+
+	btVector3 scale_with_margin;
+	G_TO_B(p_xform.basis.get_scale(), scale_with_margin);
+	btConvex->setLocalScaling(scale_with_margin);
+
+	btTransform bt_xform;
+	G_TO_B(p_xform, bt_xform);
+
+	btCollisionObject collision_object;
+	collision_object.setCollisionShape(btConvex);
+	collision_object.setWorldTransform(bt_xform);
+
+	GodotAllContactResultCallback btQuery(&collision_object, p_results, p_result_max, &p_exclude);
+	btQuery.m_collisionFilterGroup = p_collision_layer;
+	btQuery.m_collisionFilterMask = p_object_type_mask;
+	btQuery.m_closestDistanceThreshold = p_margin;
+	space->dynamicsWorld->contactTest(&collision_object, btQuery);
+
+	bulletdelete(btConvex);
+
+	return btQuery.m_count;
+}
+
+bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, ShapeRestInfo *r_info) {
+	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
+
+	btConvexShape *bt_convex_shape = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
+	if (!bt_convex_shape) {
+		bulletdelete(bt_convex_shape);
+		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
+		return 0;
+	}
+
+	btVector3 bt_motion;
+	G_TO_B(p_motion, bt_motion);
+
+	btVector3 scale_with_margin;
+	G_TO_B(p_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
+	bt_convex_shape->setLocalScaling(scale_with_margin);
+
+	btTransform bt_xform_from;
+	G_TO_B(p_xform, bt_xform_from);
+
+	btTransform bt_xform_to(bt_xform_from);
+	bt_xform_to.getOrigin() += bt_motion;
+
+	GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude);
+	btResult.m_collisionFilterGroup = p_collision_layer;
+	btResult.m_collisionFilterMask = p_object_type_mask;
+
+	space->dynamicsWorld->convexSweepTest(bt_convex_shape, bt_xform_from, bt_xform_to, btResult);
+
+	if (btResult.hasHit()) {
+		if (btCollisionObject::CO_RIGID_BODY == btResult.m_hitCollisionObject->getInternalType()) {
+			B_TO_G(static_cast<const btRigidBody *>(btResult.m_hitCollisionObject)->getVelocityInLocalPoint(btResult.m_hitPointWorld), r_info->linear_velocity);
+		}
+		CollisionObjectBullet *collision_object = static_cast<CollisionObjectBullet *>(btResult.m_hitCollisionObject->getUserPointer());
+		p_closest_safe = p_closest_unsafe = btResult.m_closestHitFraction;
+		B_TO_G(btResult.m_hitPointWorld, r_info->point);
+		B_TO_G(btResult.m_hitNormalWorld, r_info->normal);
+		r_info->rid = collision_object->get_self();
+		r_info->collider_id = collision_object->get_instance_id();
+		r_info->shape = btResult.m_shapePart;
+	}
+
+	bulletdelete(bt_convex_shape);
+	return btResult.hasHit();
+}
+
+/// Returns the list of contacts pairs in this order: Local contact, other body contact
+bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+	if (p_result_max <= 0)
+		return 0;
+
+	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
+
+	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
+	if (!btConvex) {
+		bulletdelete(btConvex);
+		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
+		return 0;
+	}
+
+	btVector3 scale_with_margin;
+	G_TO_B(p_shape_xform.basis.get_scale(), scale_with_margin);
+	btConvex->setLocalScaling(scale_with_margin);
+
+	btTransform bt_xform;
+	G_TO_B(p_shape_xform, bt_xform);
+
+	btCollisionObject collision_object;
+	collision_object.setCollisionShape(btConvex);
+	collision_object.setWorldTransform(bt_xform);
+
+	GodotContactPairContactResultCallback btQuery(&collision_object, r_results, p_result_max, &p_exclude);
+	btQuery.m_collisionFilterGroup = p_collision_layer;
+	btQuery.m_collisionFilterMask = p_object_type_mask;
+	btQuery.m_closestDistanceThreshold = p_margin;
+	space->dynamicsWorld->contactTest(&collision_object, btQuery);
+
+	r_result_count = btQuery.m_count;
+	bulletdelete(btConvex);
+
+	return btQuery.m_count;
+}
+
+bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+
+	ShapeBullet *shape = space->get_physics_server()->get_shape_owner()->get(p_shape);
+
+	btConvexShape *btConvex = dynamic_cast<btConvexShape *>(shape->create_bt_shape());
+	if (!btConvex) {
+		bulletdelete(btConvex);
+		ERR_PRINTS("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
+		return 0;
+	}
+
+	btVector3 scale_with_margin;
+	G_TO_B(p_shape_xform.basis.get_scale() + Vector3(p_margin, p_margin, p_margin), scale_with_margin);
+	btConvex->setLocalScaling(scale_with_margin);
+
+	btTransform bt_xform;
+	G_TO_B(p_shape_xform, bt_xform);
+
+	btCollisionObject collision_object;
+	collision_object.setCollisionShape(btConvex);
+	collision_object.setWorldTransform(bt_xform);
+
+	GodotRestInfoContactResultCallback btQuery(&collision_object, r_info, &p_exclude);
+	btQuery.m_collisionFilterGroup = p_collision_layer;
+	btQuery.m_collisionFilterMask = p_object_type_mask;
+	btQuery.m_closestDistanceThreshold = p_margin;
+	space->dynamicsWorld->contactTest(&collision_object, btQuery);
+
+	bulletdelete(btConvex);
+
+	if (btQuery.m_collided) {
+		if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
+			B_TO_G(static_cast<const btRigidBody *>(btQuery.m_rest_info_collision_object)->getVelocityInLocalPoint(btQuery.m_rest_info_bt_point), r_info->linear_velocity);
+		}
+		B_TO_G(btQuery.m_rest_info_bt_point, r_info->point);
+	}
+
+	return btQuery.m_collided;
+}
+
+Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
+
+	RigidCollisionObjectBullet *rigid_object = space->get_physics_server()->get_rigid_collisin_object(p_object);
+	ERR_FAIL_COND_V(!rigid_object, Vector3());
+
+	btVector3 out_closest_point(0, 0, 0);
+	btScalar out_distance = 1e20;
+
+	btVector3 bt_point;
+	G_TO_B(p_point, bt_point);
+
+	btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver;
+	btVoronoiSimplexSolver gjk_simplex_solver;
+	gjk_simplex_solver.setEqualVertexThreshold(0.);
+
+	btSphereShape point_shape(0.);
+
+	btCollisionShape *shape;
+	btConvexShape *convex_shape;
+	btTransform child_transform;
+	btTransform body_transform(rigid_object->get_bt_collision_object()->getWorldTransform());
+
+	btGjkPairDetector::ClosestPointInput input;
+	input.m_transformA.getBasis().setIdentity();
+	input.m_transformA.setOrigin(bt_point);
+
+	bool shapes_found = false;
+
+	btCompoundShape *compound = rigid_object->get_compound_shape();
+	for (int i = compound->getNumChildShapes() - 1; 0 <= i; --i) {
+		shape = compound->getChildShape(i);
+		if (shape->isConvex()) {
+			child_transform = compound->getChildTransform(i);
+			convex_shape = static_cast<btConvexShape *>(shape);
+
+			input.m_transformB = body_transform * child_transform;
+
+			btPointCollector result;
+			btGjkPairDetector gjk_pair_detector(&point_shape, convex_shape, &gjk_simplex_solver, &gjk_epa_pen_solver);
+			gjk_pair_detector.getClosestPoints(input, result, 0);
+
+			if (out_distance > result.m_distance) {
+				out_distance = result.m_distance;
+				out_closest_point = result.m_pointInWorld;
+			}
+		}
+		shapes_found = true;
+	}
+
+	if (shapes_found) {
+
+		Vector3 out;
+		B_TO_G(out_closest_point, out);
+		return out;
+	} else {
+
+		// no shapes found, use distance to origin.
+		return rigid_object->get_transform().get_origin();
+	}
+}
+
+SpaceBullet::SpaceBullet(bool p_create_soft_world)
+	: broadphase(NULL),
+	  dispatcher(NULL),
+	  solver(NULL),
+	  collisionConfiguration(NULL),
+	  dynamicsWorld(NULL),
+	  soft_body_world_info(NULL),
+	  ghostPairCallback(NULL),
+	  godotFilterCallback(NULL),
+	  gravityDirection(0, -1, 0),
+	  gravityMagnitude(10),
+	  contactDebugCount(0) {
+
+	create_empty_world(p_create_soft_world);
+	direct_access = memnew(BulletPhysicsDirectSpaceState(this));
+}
+
+SpaceBullet::~SpaceBullet() {
+	memdelete(direct_access);
+	destroy_world();
+}
+
+void SpaceBullet::flush_queries() {
+	const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
+	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
+		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
+	}
+}
+
+void SpaceBullet::step(real_t p_delta_time) {
+	dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
+}
+
+void SpaceBullet::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
+	assert(dynamicsWorld);
+
+	switch (p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY:
+			gravityMagnitude = p_value;
+			update_gravity();
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
+			gravityDirection = p_value;
+			update_gravity();
+			break;
+		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
+		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
+			break; // No damp
+		case PhysicsServer::AREA_PARAM_PRIORITY:
+			// Priority is always 0, the lower
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			break;
+		default:
+			WARN_PRINTS("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
+			break;
+	}
+}
+
+Variant SpaceBullet::get_param(PhysicsServer::AreaParameter p_param) {
+	switch (p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY:
+			return gravityMagnitude;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
+			return gravityDirection;
+		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
+		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
+			return 0; // No damp
+		case PhysicsServer::AREA_PARAM_PRIORITY:
+			return 0; // Priority is always 0, the lower
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
+			return false;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+			return 0;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			return 0;
+		default:
+			WARN_PRINTS("This get parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
+			return Variant();
+	}
+}
+
+void SpaceBullet::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
+	switch (p_param) {
+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION:
+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP:
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO:
+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
+		default:
+			WARN_PRINTS("This set parameter (" + itos(p_param) + ") is ignored, the SpaceBullet doesn't support it.");
+			break;
+	}
+}
+
+real_t SpaceBullet::get_param(PhysicsServer::SpaceParameter p_param) {
+	switch (p_param) {
+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION:
+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION:
+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD:
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD:
+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP:
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO:
+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
+		default:
+			WARN_PRINTS("The SpaceBullet  doesn't support this get parameter (" + itos(p_param) + "), 0 is returned.");
+			return 0.f;
+	}
+}
+
+void SpaceBullet::add_area(AreaBullet *p_area) {
+	areas.push_back(p_area);
+	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
+}
+
+void SpaceBullet::remove_area(AreaBullet *p_area) {
+	areas.erase(p_area);
+	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+}
+
+void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
+	// This is necessary to change collision filter
+	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
+}
+
+void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
+	if (p_body->is_static()) {
+		dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
+	} else {
+		dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
+	}
+}
+
+void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+	if (p_body->is_static()) {
+		dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
+	} else {
+		dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+	}
+}
+
+void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
+	// This is necessary to change collision filter
+	remove_rigid_body(p_body);
+	add_rigid_body(p_body);
+}
+
+void SpaceBullet::add_soft_body(SoftBodyBullet *p_body) {
+	if (is_using_soft_world()) {
+		if (p_body->get_bt_soft_body()) {
+			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->addSoftBody(p_body->get_bt_soft_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
+		}
+	} else {
+		ERR_PRINT("This soft body can't be added to non soft world");
+	}
+}
+
+void SpaceBullet::remove_soft_body(SoftBodyBullet *p_body) {
+	if (is_using_soft_world()) {
+		if (p_body->get_bt_soft_body()) {
+			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->removeSoftBody(p_body->get_bt_soft_body());
+		}
+	}
+}
+
+void SpaceBullet::reload_collision_filters(SoftBodyBullet *p_body) {
+	// This is necessary to change collision filter
+	remove_soft_body(p_body);
+	add_soft_body(p_body);
+}
+
+void SpaceBullet::add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies) {
+	p_constraint->set_space(this);
+	dynamicsWorld->addConstraint(p_constraint->get_bt_constraint(), disableCollisionsBetweenLinkedBodies);
+}
+
+void SpaceBullet::remove_constraint(ConstraintBullet *p_constraint) {
+	dynamicsWorld->removeConstraint(p_constraint->get_bt_constraint());
+}
+
+int SpaceBullet::get_num_collision_objects() const {
+	return dynamicsWorld->getNumCollisionObjects();
+}
+
+void SpaceBullet::remove_all_collision_objects() {
+	for (int i = dynamicsWorld->getNumCollisionObjects() - 1; 0 <= i; --i) {
+		btCollisionObject *btObj = dynamicsWorld->getCollisionObjectArray()[i];
+		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
+		colObj->set_space(NULL);
+	}
+}
+
+void onBulletPreTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
+	static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo())->flush_queries();
+}
+
+void onBulletTickCallback(btDynamicsWorld *p_dynamicsWorld, btScalar timeStep) {
+
+	// Notify all Collision objects the collision checker is started
+	const btCollisionObjectArray &colObjArray = p_dynamicsWorld->getCollisionObjectArray();
+	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
+		CollisionObjectBullet *colObj = static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer());
+		assert(NULL != colObj);
+		colObj->on_collision_checker_start();
+	}
+
+	SpaceBullet *sb = static_cast<SpaceBullet *>(p_dynamicsWorld->getWorldUserInfo());
+	sb->check_ghost_overlaps();
+	sb->check_body_collision();
+}
+
+BulletPhysicsDirectSpaceState *SpaceBullet::get_direct_state() {
+	return direct_access;
+}
+
+btScalar calculateGodotCombinedRestitution(const btCollisionObject *body0, const btCollisionObject *body1) {
+	return MAX(body0->getRestitution(), body1->getRestitution());
+}
+
+void SpaceBullet::create_empty_world(bool p_create_soft_world) {
+	assert(NULL == broadphase);
+	assert(NULL == dispatcher);
+	assert(NULL == solver);
+	assert(NULL == collisionConfiguration);
+	assert(NULL == dynamicsWorld);
+	assert(NULL == ghostPairCallback);
+	assert(NULL == godotFilterCallback);
+
+	void *world_mem;
+	if (p_create_soft_world) {
+		world_mem = malloc(sizeof(btSoftRigidDynamicsWorld));
+	} else {
+		world_mem = malloc(sizeof(btDiscreteDynamicsWorld));
+	}
+
+	if (p_create_soft_world) {
+		collisionConfiguration = bulletnew(btSoftBodyRigidBodyCollisionConfiguration);
+	} else {
+		collisionConfiguration = bulletnew(GodotCollisionConfiguration(static_cast<btDiscreteDynamicsWorld *>(world_mem)));
+	}
+
+	dispatcher = bulletnew(GodotCollisionDispatcher(collisionConfiguration));
+	broadphase = bulletnew(btDbvtBroadphase);
+	solver = bulletnew(btSequentialImpulseConstraintSolver);
+
+	if (p_create_soft_world) {
+		dynamicsWorld = new (world_mem) btSoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
+		soft_body_world_info = bulletnew(btSoftBodyWorldInfo);
+	} else {
+		dynamicsWorld = new (world_mem) btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
+	}
+
+	ghostPairCallback = bulletnew(btGhostPairCallback);
+	godotFilterCallback = bulletnew(GodotFilterCallback);
+	gCalculateCombinedRestitutionCallback = &calculateGodotCombinedRestitution;
+
+	dynamicsWorld->setWorldUserInfo(this);
+
+	dynamicsWorld->setInternalTickCallback(onBulletPreTickCallback, this, true);
+	dynamicsWorld->setInternalTickCallback(onBulletTickCallback, this, false);
+	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(ghostPairCallback); // Setup ghost check
+	dynamicsWorld->getPairCache()->setOverlapFilterCallback(godotFilterCallback);
+
+	if (soft_body_world_info) {
+		soft_body_world_info->m_broadphase = broadphase;
+		soft_body_world_info->m_dispatcher = dispatcher;
+		soft_body_world_info->m_sparsesdf.Initialize();
+	}
+
+	update_gravity();
+}
+
+void SpaceBullet::destroy_world() {
+	assert(NULL != broadphase);
+	assert(NULL != dispatcher);
+	assert(NULL != solver);
+	assert(NULL != collisionConfiguration);
+	assert(NULL != dynamicsWorld);
+	assert(NULL != ghostPairCallback);
+	assert(NULL != godotFilterCallback);
+
+	/// The world elements (like: Collision Objects, Constraints, Shapes) are managed by godot
+
+	dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(NULL);
+	dynamicsWorld->getPairCache()->setOverlapFilterCallback(NULL);
+
+	bulletdelete(ghostPairCallback);
+	bulletdelete(godotFilterCallback);
+
+	// Deallocate world
+	dynamicsWorld->~btDiscreteDynamicsWorld();
+	free(dynamicsWorld);
+	dynamicsWorld = NULL;
+
+	bulletdelete(solver);
+	bulletdelete(broadphase);
+	bulletdelete(dispatcher);
+	bulletdelete(collisionConfiguration);
+	bulletdelete(soft_body_world_info);
+}
+
+void SpaceBullet::check_ghost_overlaps() {
+
+	/// Algorith support variables
+	btGjkEpaPenetrationDepthSolver gjk_epa_pen_solver;
+	btVoronoiSimplexSolver gjk_simplex_solver;
+	gjk_simplex_solver.setEqualVertexThreshold(0.f);
+	btConvexShape *other_body_shape;
+	btConvexShape *area_shape;
+	btGjkPairDetector::ClosestPointInput gjk_input;
+	AreaBullet *area;
+	RigidCollisionObjectBullet *otherObject;
+	int x(-1), i(-1), y(-1), z(-1), indexOverlap(-1);
+
+	/// For each areas
+	for (x = areas.size() - 1; 0 <= x; --x) {
+		area = areas[x];
+
+		if (!area->is_monitoring())
+			continue;
+
+		/// 1. Reset all states
+		for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
+			AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects[i];
+			// This check prevent the overwrite of ENTER state
+			// if this function is called more times before dispatchCallbacks
+			if (otherObj.state != AreaBullet::OVERLAP_STATE_ENTER) {
+				otherObj.state = AreaBullet::OVERLAP_STATE_DIRTY;
+			}
+		}
+
+		/// 2. Check all overlapping objects using GJK
+
+		const btAlignedObjectArray<btCollisionObject *> ghostOverlaps = area->get_bt_ghost()->getOverlappingPairs();
+
+		// For each overlapping
+		for (i = ghostOverlaps.size() - 1; 0 <= i; --i) {
+
+			if (!(ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_RIGID_BODY || ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA))
+				continue;
+
+			otherObject = static_cast<RigidCollisionObjectBullet *>(ghostOverlaps[i]->getUserPointer());
+
+			bool hasOverlap = false;
+
+			// For each area shape
+			for (y = area->get_compound_shape()->getNumChildShapes() - 1; 0 <= y; --y) {
+				if (!area->get_compound_shape()->getChildShape(y)->isConvex())
+					continue;
+
+				gjk_input.m_transformA = area->get_transform__bullet() * area->get_compound_shape()->getChildTransform(y);
+				area_shape = static_cast<btConvexShape *>(area->get_compound_shape()->getChildShape(y));
+
+				// For each other object shape
+				for (z = otherObject->get_compound_shape()->getNumChildShapes() - 1; 0 <= z; --z) {
+
+					if (!otherObject->get_compound_shape()->getChildShape(z)->isConvex())
+						continue;
+
+					other_body_shape = static_cast<btConvexShape *>(otherObject->get_compound_shape()->getChildShape(z));
+					gjk_input.m_transformB = otherObject->get_transform__bullet() * otherObject->get_compound_shape()->getChildTransform(z);
+
+					btPointCollector result;
+					btGjkPairDetector gjk_pair_detector(area_shape, other_body_shape, &gjk_simplex_solver, &gjk_epa_pen_solver);
+					gjk_pair_detector.getClosestPoints(gjk_input, result, 0);
+
+					if (0 >= result.m_distance) {
+						hasOverlap = true;
+						goto collision_found;
+					}
+				} // ~For each other object shape
+			} // ~For each area shape
+
+		collision_found:
+			if (!hasOverlap)
+				continue;
+
+			indexOverlap = area->find_overlapping_object(otherObject);
+			if (-1 == indexOverlap) {
+				// Not found
+				area->add_overlap(otherObject);
+			} else {
+				// Found
+				area->put_overlap_as_inside(indexOverlap);
+			}
+		}
+
+		/// 3. Remove not overlapping
+		for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
+			// If the overlap has DIRTY state it means that it's no more overlapping
+			if (area->overlappingObjects[i].state == AreaBullet::OVERLAP_STATE_DIRTY) {
+				area->put_overlap_as_exit(i);
+			}
+		}
+	}
+}
+
+void SpaceBullet::check_body_collision() {
+#ifdef DEBUG_ENABLED
+	reset_debug_contact_count();
+#endif
+
+	const int numManifolds = dynamicsWorld->getDispatcher()->getNumManifolds();
+	for (int i = 0; i < numManifolds; ++i) {
+		btPersistentManifold *contactManifold = dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
+		const btCollisionObject *obA = contactManifold->getBody0();
+		const btCollisionObject *obB = contactManifold->getBody1();
+
+		if (btCollisionObject::CO_RIGID_BODY != obA->getInternalType() || btCollisionObject::CO_RIGID_BODY != obB->getInternalType()) {
+			// This checks is required to be sure the ghost object is skipped
+			// The ghost object "getUserPointer" return the BodyBullet owner so this check is required
+			continue;
+		}
+
+		// Asserts all Godot objects are assigned
+		assert(NULL != obA->getUserPointer());
+		assert(NULL != obB->getUserPointer());
+
+		// I know this static cast is a bit risky. But I'm checking its type just after it.
+		// This allow me to avoid a lot of other cast and checks
+		RigidBodyBullet *bodyA = static_cast<RigidBodyBullet *>(obA->getUserPointer());
+		RigidBodyBullet *bodyB = static_cast<RigidBodyBullet *>(obB->getUserPointer());
+
+		if (CollisionObjectBullet::TYPE_RIGID_BODY == bodyA->getType() && CollisionObjectBullet::TYPE_RIGID_BODY == bodyB->getType()) {
+			if (!bodyA->can_add_collision() && !bodyB->can_add_collision()) {
+				continue;
+			}
+
+			const int numContacts = contactManifold->getNumContacts();
+#define REPORT_ALL_CONTACTS 0
+#if REPORT_ALL_CONTACTS
+			for (int j = 0; j < numContacts; j++) {
+				btManifoldPoint &pt = contactManifold->getContactPoint(j);
+#else
+			// Since I don't need report all contacts for these objects, I'll report only the first
+			if (numContacts) {
+				btManifoldPoint &pt = contactManifold->getContactPoint(0);
+#endif
+				Vector3 collisionWorldPosition;
+				Vector3 collisionLocalPosition;
+				Vector3 normalOnB;
+				B_TO_G(pt.m_normalWorldOnB, normalOnB);
+
+				if (bodyA->can_add_collision()) {
+					B_TO_G(pt.getPositionWorldOnB(), collisionWorldPosition);
+					/// pt.m_localPointB Doesn't report the exact point in local space
+					B_TO_G(pt.getPositionWorldOnB() - obB->getWorldTransform().getOrigin(), collisionLocalPosition);
+					bodyA->add_collision_object(bodyB, collisionWorldPosition, collisionLocalPosition, normalOnB, pt.m_index1, pt.m_index0);
+				}
+				if (bodyB->can_add_collision()) {
+					B_TO_G(pt.getPositionWorldOnA(), collisionWorldPosition);
+					/// pt.m_localPointA Doesn't report the exact point in local space
+					B_TO_G(pt.getPositionWorldOnA() - obA->getWorldTransform().getOrigin(), collisionLocalPosition);
+					bodyB->add_collision_object(bodyA, collisionWorldPosition, collisionLocalPosition, normalOnB * -1, pt.m_index0, pt.m_index1);
+				}
+
+#ifdef DEBUG_ENABLED
+				if (is_debugging_contacts()) {
+					add_debug_contact(collisionWorldPosition);
+				}
+#endif
+			}
+		}
+	}
+}
+
+void SpaceBullet::update_gravity() {
+	btVector3 btGravity;
+	G_TO_B(gravityDirection * gravityMagnitude, btGravity);
+	dynamicsWorld->setGravity(btGravity);
+	if (soft_body_world_info) {
+		soft_body_world_info->m_gravity = btGravity;
+	}
+}
+
+/// IMPORTANT: Please don't turn it ON this is not managed correctly!!
+/// I'm leaving this here just for future tests.
+/// Debug motion and normal vector drawing
+#define debug_test_motion 0
+#if debug_test_motion
+static ImmediateGeometry *motionVec(NULL);
+static ImmediateGeometry *normalLine(NULL);
+static Ref<SpatialMaterial> red_mat;
+static Ref<SpatialMaterial> blue_mat;
+#endif
+
+#define IGNORE_AREAS_TRUE true
+bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) {
+
+#if debug_test_motion
+	/// Yes I know this is not good, but I've used it as fast debugging.
+	/// I'm leaving it here just for speedup the other eventual debugs
+	if (!normalLine) {
+		motionVec = memnew(ImmediateGeometry);
+		normalLine = memnew(ImmediateGeometry);
+		SceneTree::get_singleton()->get_current_scene()->add_child(motionVec);
+		SceneTree::get_singleton()->get_current_scene()->add_child(normalLine);
+
+		red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
+		red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
+		red_mat->set_line_width(20.0);
+		red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
+		red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+		red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
+		red_mat->set_albedo(Color(1, 0, 0, 1));
+		motionVec->set_material_override(red_mat);
+
+		blue_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
+		blue_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
+		blue_mat->set_line_width(20.0);
+		blue_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
+		blue_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+		blue_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
+		blue_mat->set_albedo(Color(0, 0, 1, 1));
+		normalLine->set_material_override(blue_mat);
+	}
+#endif
+
+	///// Release all generated manifolds
+	//{
+	//    if(p_body->get_kinematic_utilities()){
+	//        for(int i= p_body->get_kinematic_utilities()->m_generatedManifold.size()-1; 0<=i; --i){
+	//            dispatcher->releaseManifold( p_body->get_kinematic_utilities()->m_generatedManifold[i] );
+	//        }
+	//        p_body->get_kinematic_utilities()->m_generatedManifold.clear();
+	//    }
+	//}
+
+	btVector3 recover_initial_position;
+	recover_initial_position.setZero();
+
+/// I'm performing the unstack at the end of movement so I'm sure the player is unstacked even after the movement.
+///  I've removed the initial unstack because this is useful just for the first tick since after the first
+///  the real unstack is performed at the end of process.
+/// However I'm leaving here the old code.
+///  Note: It has a bug when two shapes touches something simultaneously the body is moved too much away (I'm not fixing it for the reason written above).
+#define INITIAL_UNSTACK 0
+#if !INITIAL_UNSTACK
+	btTransform body_safe_position;
+	G_TO_B(p_from, body_safe_position);
+//btTransform body_unsafe_positino;
+//G_TO_B(p_from, body_unsafe_positino);
+#else
+	btTransform body_safe_position;
+	btTransform body_unsafe_positino;
+	{ /// Phase one - multi shapes depenetration using margin
+		G_TO_B(p_from, body_safe_position);
+		G_TO_B(p_from, body_unsafe_positino);
+
+		// MAX_PENETRATION_DEPTH Is useful have the ghost a bit penetrated so I can detect the floor easily
+		recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */ 1, recover_initial_position);
+
+		/// Not required if I put p_depenetration_speed = 1
+		//for(int t = 0; t<4; ++t){
+		//    if(!recover_from_penetration(p_body, body_safe_position, MAX_PENETRATION_DEPTH, /* p_depenetration_speed */0.2, recover_initial_position)){
+		//       break;
+		//    }
+		//}
+
+		// Add recover position to "From" and "To" transforms
+		body_safe_position.getOrigin() += recover_initial_position;
+	}
+#endif
+
+	int shape_most_recovered(-1);
+	btVector3 recovered_motion;
+	G_TO_B(p_motion, recovered_motion);
+	const int shape_count(p_body->get_shape_count());
+
+	{ /// phase two - sweep test, from a secure position without margin
+
+#if debug_test_motion
+		Vector3 sup_line;
+		B_TO_G(body_safe_position.getOrigin(), sup_line);
+		motionVec->clear();
+		motionVec->begin(Mesh::PRIMITIVE_LINES, NULL);
+		motionVec->add_vertex(sup_line);
+		motionVec->add_vertex(sup_line + p_motion * 10);
+		motionVec->end();
+#endif
+
+		for (int shIndex = 0; shIndex < shape_count; ++shIndex) {
+			if (p_body->is_shape_disabled(shIndex)) {
+				continue;
+			}
+
+			btConvexShape *convex_shape_test(dynamic_cast<btConvexShape *>(p_body->get_bt_shape(shIndex)));
+			if (!convex_shape_test) {
+				// Skip no convex shape
+				continue;
+			}
+
+			btTransform shape_xform_from;
+			G_TO_B(p_body->get_shape_transform(shIndex), shape_xform_from);
+			//btTransform shape_xform_to(shape_xform_from);
+
+			// Add local shape transform
+			shape_xform_from.getOrigin() += body_safe_position.getOrigin();
+			shape_xform_from.getBasis() *= body_safe_position.getBasis();
+
+			btTransform shape_xform_to(shape_xform_from);
+			//shape_xform_to.getOrigin() += body_unsafe_positino.getOrigin();
+			//shape_xform_to.getBasis() *= body_unsafe_positino.getBasis();
+			shape_xform_to.getOrigin() += recovered_motion;
+
+			GodotKinClosestConvexResultCallback btResult(shape_xform_from.getOrigin(), shape_xform_to.getOrigin(), p_body, IGNORE_AREAS_TRUE);
+			btResult.m_collisionFilterGroup = p_body->get_collision_layer();
+			btResult.m_collisionFilterMask = p_body->get_collision_mask();
+
+			dynamicsWorld->convexSweepTest(convex_shape_test, shape_xform_from, shape_xform_to, btResult);
+
+			if (btResult.hasHit()) {
+				//recovered_motion *= btResult.m_closestHitFraction;
+				/// Since for each sweep test I fix the motion of new shapes in base the recover result,
+				/// if another shape will hit something it means that has a deepest recovering respect the previous shape
+				shape_most_recovered = shIndex;
+			}
+		}
+	}
+
+	bool hasHit = false;
+
+	{ /// Phase three - contact test with margin
+
+		btGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject;
+
+		GodotRecoverAndClosestContactResultCallback result_callabck;
+
+		if (false && 0 <= shape_most_recovered) {
+			result_callabck.m_self_object = p_body;
+			result_callabck.m_ignore_areas = IGNORE_AREAS_TRUE;
+			result_callabck.m_collisionFilterGroup = p_body->get_collision_layer();
+			result_callabck.m_collisionFilterMask = p_body->get_collision_mask();
+
+			const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[shape_most_recovered]);
+			ghost->setCollisionShape(kin.shape);
+			ghost->setWorldTransform(body_safe_position);
+
+			ghost->getWorldTransform().getOrigin() += recovered_motion;
+			ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin();
+			ghost->getWorldTransform().getBasis() *= kin.transform.getBasis();
+
+			dynamicsWorld->contactTest(ghost, result_callabck);
+
+			recovered_motion += result_callabck.m_recover_penetration; // Required to avoid all kind of penetration
+
+		} else {
+			// The sweep result does not return a penetrated shape, so I've to check all shapes
+			// Then return the most penetrated shape
+
+			GodotRecoverAndClosestContactResultCallback iter_result_callabck(p_body, IGNORE_AREAS_TRUE);
+			iter_result_callabck.m_collisionFilterGroup = p_body->get_collision_layer();
+			iter_result_callabck.m_collisionFilterMask = p_body->get_collision_mask();
+
+			btScalar max_penetration(99999999999);
+			for (int i = 0; i < shape_count; ++i) {
+
+				const RigidBodyBullet::KinematicShape &kin(p_body->get_kinematic_utilities()->m_shapes[i]);
+				if (!kin.is_active()) {
+					continue;
+				}
+
+				// reset callback each function
+				iter_result_callabck.reset();
+
+				ghost->setCollisionShape(kin.shape);
+				ghost->setWorldTransform(body_safe_position);
+				ghost->getWorldTransform().getOrigin() += recovered_motion;
+				ghost->getWorldTransform().getOrigin() += kin.transform.getOrigin();
+				ghost->getWorldTransform().getBasis() *= kin.transform.getBasis();
+
+				dynamicsWorld->contactTest(ghost, iter_result_callabck);
+
+				if (iter_result_callabck.hasHit()) {
+					if (max_penetration > iter_result_callabck.m_penetration_distance) {
+						max_penetration = iter_result_callabck.m_penetration_distance;
+						shape_most_recovered = i;
+						// This is more penetrated
+						result_callabck.m_pointCollisionObject = iter_result_callabck.m_pointCollisionObject;
+						result_callabck.m_pointNormalWorld = iter_result_callabck.m_pointNormalWorld;
+						result_callabck.m_pointWorld = iter_result_callabck.m_pointWorld;
+						result_callabck.m_penetration_distance = iter_result_callabck.m_penetration_distance;
+						result_callabck.m_other_compound_shape_index = iter_result_callabck.m_other_compound_shape_index;
+
+						recovered_motion += iter_result_callabck.m_recover_penetration; // Required to avoid all kind of penetration
+					}
+				}
+			}
+		}
+
+		hasHit = result_callabck.hasHit();
+
+		if (r_result) {
+
+			B_TO_G(recovered_motion + recover_initial_position, r_result->motion);
+
+			if (hasHit) {
+
+				if (btCollisionObject::CO_RIGID_BODY != result_callabck.m_pointCollisionObject->getInternalType()) {
+					ERR_PRINT("The collision is not against a rigid body. Please check what's going on.");
+					goto EndExecution;
+				}
+				const btRigidBody *btRigid = static_cast<const btRigidBody *>(result_callabck.m_pointCollisionObject);
+				CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(btRigid->getUserPointer());
+
+				r_result->remainder = p_motion - r_result->motion; // is the remaining movements
+				B_TO_G(result_callabck.m_pointWorld, r_result->collision_point);
+				B_TO_G(result_callabck.m_pointNormalWorld, r_result->collision_normal);
+				B_TO_G(btRigid->getVelocityInLocalPoint(result_callabck.m_pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); // It calculates velocity at point and assign it using special function Bullet_to_Godot
+				r_result->collider = collisionObject->get_self();
+				r_result->collider_id = collisionObject->get_instance_id();
+				r_result->collider_shape = result_callabck.m_other_compound_shape_index;
+				r_result->collision_local_shape = shape_most_recovered;
+
+//{ /// Add manifold point to manage collisions
+//    btPersistentManifold* manifold = dynamicsWorld->getDispatcher()->getNewManifold(p_body->getBtBody(), btRigid);
+//    btManifoldPoint manifoldPoint(result_callabck.m_pointWorld, result_callabck.m_pointWorld, result_callabck.m_pointNormalWorld, result_callabck.m_penetration_distance);
+//    manifoldPoint.m_index0 = r_result->collision_local_shape;
+//    manifoldPoint.m_index1 = r_result->collider_shape;
+//    manifold->addManifoldPoint(manifoldPoint);
+//    p_body->get_kinematic_utilities()->m_generatedManifold.push_back(manifold);
+//}
+
+#if debug_test_motion
+				Vector3 sup_line2;
+				B_TO_G(recovered_motion, sup_line2);
+				//Vector3 sup_pos;
+				//B_TO_G( pt.getPositionWorldOnB(), sup_pos);
+				normalLine->clear();
+				normalLine->begin(Mesh::PRIMITIVE_LINES, NULL);
+				normalLine->add_vertex(r_result->collision_point);
+				normalLine->add_vertex(r_result->collision_point + r_result->collision_normal * 10);
+				normalLine->end();
+#endif
+
+			} else {
+				r_result->remainder = Vector3();
+			}
+		}
+	}
+
+EndExecution:
+	p_body->get_kinematic_utilities()->resetDefShape();
+	return hasHit;
+}
+
+///  Note: It has a bug when two shapes touches something simultaneously the body is moved too much away
+/// (I'm not fixing it because I don't use it).
+bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position) {
+
+	bool penetration = false;
+	btPairCachingGhostObject *ghost = p_body->get_kinematic_utilities()->m_ghostObject;
+
+	for (int kinIndex = p_body->get_kinematic_utilities()->m_shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
+		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->m_shapes[kinIndex]);
+		if (!kin_shape.is_active()) {
+			continue;
+		}
+
+		btConvexShape *convexShape = kin_shape.shape;
+		btTransform shape_xform(kin_shape.transform);
+
+		// from local to world
+		shape_xform.getOrigin() += p_from.getOrigin();
+		shape_xform.getBasis() *= p_from.getBasis();
+
+		// Apply last recovery to avoid doubling the recovering
+		shape_xform.getOrigin() += out_recover_position;
+
+		ghost->setCollisionShape(convexShape);
+		ghost->setWorldTransform(shape_xform);
+
+		btVector3 minAabb, maxAabb;
+		convexShape->getAabb(shape_xform, minAabb, maxAabb);
+		dynamicsWorld->getBroadphase()->setAabb(ghost->getBroadphaseHandle(),
+				minAabb,
+				maxAabb,
+				dynamicsWorld->getDispatcher());
+
+		dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs(ghost->getOverlappingPairCache(), dynamicsWorld->getDispatchInfo(), dynamicsWorld->getDispatcher());
+
+		for (int i = 0; i < ghost->getOverlappingPairCache()->getNumOverlappingPairs(); ++i) {
+			p_body->get_kinematic_utilities()->m_manifoldArray.resize(0);
+
+			btBroadphasePair *collisionPair = &ghost->getOverlappingPairCache()->getOverlappingPairArray()[i];
+
+			btCollisionObject *obj0 = static_cast<btCollisionObject *>(collisionPair->m_pProxy0->m_clientObject);
+			btCollisionObject *obj1 = static_cast<btCollisionObject *>(collisionPair->m_pProxy1->m_clientObject);
+
+			if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
+				continue;
+
+			// This is not required since the dispatched does all the job
+			//if (!needsCollision(obj0, obj1))
+			//    continue;
+
+			if (collisionPair->m_algorithm)
+				collisionPair->m_algorithm->getAllContactManifolds(p_body->get_kinematic_utilities()->m_manifoldArray);
+
+			for (int j = 0; j < p_body->get_kinematic_utilities()->m_manifoldArray.size(); ++j) {
+
+				btPersistentManifold *manifold = p_body->get_kinematic_utilities()->m_manifoldArray[j];
+				btScalar directionSign = manifold->getBody0() == ghost ? btScalar(-1.0) : btScalar(1.0);
+				for (int p = 0; p < manifold->getNumContacts(); ++p) {
+					const btManifoldPoint &pt = manifold->getContactPoint(p);
+
+					btScalar dist = pt.getDistance();
+					if (dist < -p_maxPenetrationDepth) {
+						penetration = true;
+						out_recover_position += pt.m_normalWorldOnB * directionSign * (dist + p_maxPenetrationDepth) * p_depenetration_speed;
+						//print_line("penetrate distance: " + rtos(dist));
+					}
+					//else {
+					//    print_line("touching distance: " + rtos(dist));
+					//}
+				}
+			}
+		}
+	}
+
+	p_body->get_kinematic_utilities()->resetDefShape();
+	return penetration;
+}

modules/bullet/space_bullet.h

@@ -0,0 +1,176 @@
+/*************************************************************************/
+/*  space_bullet.h                                                       */
+/*  Author: AndreaCatania                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef SPACE_BULLET_H
+#define SPACE_BULLET_H
+
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "core/variant.h"
+#include "core/vector.h"
+#include "godot_result_callbacks.h"
+#include "rid_bullet.h"
+#include "servers/physics_server.h"
+
+class AreaBullet;
+class btBroadphaseInterface;
+class btCollisionDispatcher;
+class btConstraintSolver;
+class btDefaultCollisionConfiguration;
+class btDynamicsWorld;
+class btDiscreteDynamicsWorld;
+class btEmptyShape;
+class btGhostPairCallback;
+class btSoftRigidDynamicsWorld;
+class btSoftBodyWorldInfo;
+class ConstraintBullet;
+class CollisionObjectBullet;
+class RigidBodyBullet;
+class SpaceBullet;
+class SoftBodyBullet;
+
+class BulletPhysicsDirectSpaceState : public PhysicsDirectSpaceState {
+	GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState)
+private:
+	SpaceBullet *space;
+
+public:
+	BulletPhysicsDirectSpaceState(SpaceBullet *p_space);
+
+	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false);
+	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL);
+	/// Returns the list of contacts pairs in this order: Local contact, other body contact
+	virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const;
+};
+
+class SpaceBullet : public RIDBullet {
+private:
+	friend class AreaBullet;
+	friend void onBulletTickCallback(btDynamicsWorld *world, btScalar timeStep);
+	friend class BulletPhysicsDirectSpaceState;
+
+	btBroadphaseInterface *broadphase;
+	btDefaultCollisionConfiguration *collisionConfiguration;
+	btCollisionDispatcher *dispatcher;
+	btConstraintSolver *solver;
+	btDiscreteDynamicsWorld *dynamicsWorld;
+	btGhostPairCallback *ghostPairCallback;
+	GodotFilterCallback *godotFilterCallback;
+	btSoftBodyWorldInfo *soft_body_world_info;
+
+	BulletPhysicsDirectSpaceState *direct_access;
+	Vector3 gravityDirection;
+	real_t gravityMagnitude;
+
+	Vector<AreaBullet *> areas;
+
+	Vector<Vector3> contactDebug;
+	int contactDebugCount;
+
+public:
+	SpaceBullet(bool p_create_soft_world);
+	virtual ~SpaceBullet();
+
+	void flush_queries();
+	void step(real_t p_delta_time);
+
+	_FORCE_INLINE_ btCollisionDispatcher *get_dispatcher() { return dispatcher; }
+	_FORCE_INLINE_ btSoftBodyWorldInfo *get_soft_body_world_info() { return soft_body_world_info; }
+	_FORCE_INLINE_ bool is_using_soft_world() { return soft_body_world_info; }
+
+	/// Used to set some parameters to Bullet world
+	/// @param p_param:
+	///     AREA_PARAM_GRAVITY          to set the gravity magnitude of entire world
+	///     AREA_PARAM_GRAVITY_VECTOR   to set the gravity direction of entire world
+	void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value);
+	/// Used to get some parameters to Bullet world
+	/// @param p_param:
+	///     AREA_PARAM_GRAVITY          to get the gravity magnitude of entire world
+	///     AREA_PARAM_GRAVITY_VECTOR   to get the gravity direction of entire world
+	Variant get_param(PhysicsServer::AreaParameter p_param);
+
+	void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
+	real_t get_param(PhysicsServer::SpaceParameter p_param);
+
+	void add_area(AreaBullet *p_area);
+	void remove_area(AreaBullet *p_area);
+	void reload_collision_filters(AreaBullet *p_area);
+
+	void add_rigid_body(RigidBodyBullet *p_body);
+	void remove_rigid_body(RigidBodyBullet *p_body);
+	void reload_collision_filters(RigidBodyBullet *p_body);
+
+	void add_soft_body(SoftBodyBullet *p_body);
+	void remove_soft_body(SoftBodyBullet *p_body);
+	void reload_collision_filters(SoftBodyBullet *p_body);
+
+	void add_constraint(ConstraintBullet *p_constraint, bool disableCollisionsBetweenLinkedBodies = false);
+	void remove_constraint(ConstraintBullet *p_constraint);
+
+	int get_num_collision_objects() const;
+	void remove_all_collision_objects();
+
+	BulletPhysicsDirectSpaceState *get_direct_state();
+
+	void set_debug_contacts(int p_amount) { contactDebug.resize(p_amount); }
+	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contactDebug.empty(); }
+	_FORCE_INLINE_ void reset_debug_contact_count() {
+		contactDebugCount = 0;
+	}
+	_FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) {
+		if (contactDebugCount < contactDebug.size()) contactDebug[contactDebugCount++] = p_contact;
+	}
+	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contactDebug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contactDebugCount; }
+
+	const Vector3 &get_gravity_direction() const { return gravityDirection; }
+	real_t get_gravity_magnitude() const { return gravityMagnitude; }
+
+	void update_gravity();
+
+	bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result);
+
+private:
+	void create_empty_world(bool p_create_soft_world);
+	void destroy_world();
+	void check_ghost_overlaps();
+	void check_body_collision();
+
+	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_from, btScalar p_maxPenetrationDepth, btScalar p_depenetration_speed, btVector3 &out_recover_position);
+};
+#endif

servers/physics_server.h

@@ -357,7 +357,7 @@ public:
 		BODY_MODE_STATIC,
 		BODY_MODE_KINEMATIC,
 		BODY_MODE_RIGID,
-		//BODY_MODE_SOFT
+		BODY_MODE_SOFT,
 		BODY_MODE_CHARACTER
 	};
 

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h

@@ -0,0 +1,40 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_BROADPHASE_CALLBACK_H
+#define B3_BROADPHASE_CALLBACK_H
+
+#include "Bullet3Common/b3Vector3.h"
+struct b3BroadphaseProxy;
+
+
+struct	b3BroadphaseAabbCallback
+{
+	virtual ~b3BroadphaseAabbCallback() {}
+	virtual bool	process(const b3BroadphaseProxy* proxy) = 0;
+};
+
+
+struct	b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
+{
+	///added some cached data to accelerate ray-AABB tests
+	b3Vector3		m_rayDirectionInverse;
+	unsigned int	m_signs[3];
+	b3Scalar		m_lambda_max;
+
+	virtual ~b3BroadphaseRayCallback() {}
+};
+
+#endif //B3_BROADPHASE_CALLBACK_H

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp

@@ -0,0 +1,1325 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///b3DynamicBvh implementation by Nathanael Presson
+
+#include "b3DynamicBvh.h"
+
+//
+typedef b3AlignedObjectArray<b3DbvtNode*>			b3NodeArray;
+typedef b3AlignedObjectArray<const b3DbvtNode*>	b3ConstNodeArray;
+
+//
+struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide
+{
+	b3ConstNodeArray	nodes;
+	void Process(const b3DbvtNode* n) { nodes.push_back(n); }
+};
+
+//
+static B3_DBVT_INLINE int			b3IndexOf(const b3DbvtNode* node)
+{
+	return(node->parent->childs[1]==node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtVolume	b3Merge(	const b3DbvtVolume& a,
+									  const b3DbvtVolume& b)
+{
+#if (B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE)
+	B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]);
+	b3DbvtVolume&	res=*(b3DbvtVolume*)locals;
+#else
+		b3DbvtVolume	res;
+#endif
+	b3Merge(a,b,res);
+	return(res);
+}
+
+// volume+edge lengths
+static B3_DBVT_INLINE b3Scalar		b3Size(const b3DbvtVolume& a)
+{
+	const b3Vector3	edges=a.Lengths();
+	return(	edges.x*edges.y*edges.z+
+		edges.x+edges.y+edges.z);
+}
+
+//
+static void						b3GetMaxDepth(const b3DbvtNode* node,int depth,int& maxdepth)
+{
+	if(node->isinternal())
+	{
+		b3GetMaxDepth(node->childs[0],depth+1,maxdepth);
+		b3GetMaxDepth(node->childs[1],depth+1,maxdepth);
+	} else maxdepth=b3Max(maxdepth,depth);
+}
+
+//
+static B3_DBVT_INLINE void			b3DeleteNode(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* node)
+{
+	b3AlignedFree(pdbvt->m_free);
+	pdbvt->m_free=node;
+}
+
+//
+static void						b3RecurseDeleteNode(	b3DynamicBvh* pdbvt,
+												  b3DbvtNode* node)
+{
+	if(!node->isleaf())
+	{
+		b3RecurseDeleteNode(pdbvt,node->childs[0]);
+		b3RecurseDeleteNode(pdbvt,node->childs[1]);
+	}
+	if(node==pdbvt->m_root) pdbvt->m_root=0;
+	b3DeleteNode(pdbvt,node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode*	b3CreateNode(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* parent,
+										   void* data)
+{
+	b3DbvtNode*	node;
+	if(pdbvt->m_free)
+	{ node=pdbvt->m_free;pdbvt->m_free=0; }
+	else
+	{ node=new(b3AlignedAlloc(sizeof(b3DbvtNode),16)) b3DbvtNode(); }
+	node->parent	=	parent;
+	node->data		=	data;
+	node->childs[1]	=	0;
+	return(node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode*	b3CreateNode(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* parent,
+										   const b3DbvtVolume& volume,
+										   void* data)
+{
+	b3DbvtNode*	node=b3CreateNode(pdbvt,parent,data);
+	node->volume=volume;
+	return(node);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode*	b3CreateNode(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* parent,
+										   const b3DbvtVolume& volume0,
+										   const b3DbvtVolume& volume1,
+										   void* data)
+{
+	b3DbvtNode*	node=b3CreateNode(pdbvt,parent,data);
+	b3Merge(volume0,volume1,node->volume);
+	return(node);
+}
+
+//
+static void						b3InsertLeaf(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* root,
+										   b3DbvtNode* leaf)
+{
+	if(!pdbvt->m_root)
+	{
+		pdbvt->m_root	=	leaf;
+		leaf->parent	=	0;
+	}
+	else
+	{
+		if(!root->isleaf())
+		{
+			do	{
+				root=root->childs[b3Select(	leaf->volume,
+					root->childs[0]->volume,
+					root->childs[1]->volume)];
+			} while(!root->isleaf());
+		}
+		b3DbvtNode*	prev=root->parent;
+		b3DbvtNode*	node=b3CreateNode(pdbvt,prev,leaf->volume,root->volume,0);
+		if(prev)
+		{
+			prev->childs[b3IndexOf(root)]	=	node;
+			node->childs[0]				=	root;root->parent=node;
+			node->childs[1]				=	leaf;leaf->parent=node;
+			do	{
+				if(!prev->volume.Contain(node->volume))
+					b3Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+				else
+					break;
+				node=prev;
+			} while(0!=(prev=node->parent));
+		}
+		else
+		{
+			node->childs[0]	=	root;root->parent=node;
+			node->childs[1]	=	leaf;leaf->parent=node;
+			pdbvt->m_root	=	node;
+		}
+	}
+}
+
+//
+static b3DbvtNode*				b3RemoveLeaf(	b3DynamicBvh* pdbvt,
+										   b3DbvtNode* leaf)
+{
+	if(leaf==pdbvt->m_root)
+	{
+		pdbvt->m_root=0;
+		return(0);
+	}
+	else
+	{
+		b3DbvtNode*	parent=leaf->parent;
+		b3DbvtNode*	prev=parent->parent;
+		b3DbvtNode*	sibling=parent->childs[1-b3IndexOf(leaf)];			
+		if(prev)
+		{
+			prev->childs[b3IndexOf(parent)]=sibling;
+			sibling->parent=prev;
+			b3DeleteNode(pdbvt,parent);
+			while(prev)
+			{
+				const b3DbvtVolume	pb=prev->volume;
+				b3Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+				if(b3NotEqual(pb,prev->volume))
+				{
+					prev=prev->parent;
+				} else break;
+			}
+			return(prev?prev:pdbvt->m_root);
+		}
+		else
+		{								
+			pdbvt->m_root=sibling;
+			sibling->parent=0;
+			b3DeleteNode(pdbvt,parent);
+			return(pdbvt->m_root);
+		}			
+	}
+}
+
+//
+static void						b3FetchLeaves(b3DynamicBvh* pdbvt,
+											b3DbvtNode* root,
+											b3NodeArray& leaves,
+											int depth=-1)
+{
+	if(root->isinternal()&&depth)
+	{
+		b3FetchLeaves(pdbvt,root->childs[0],leaves,depth-1);
+		b3FetchLeaves(pdbvt,root->childs[1],leaves,depth-1);
+		b3DeleteNode(pdbvt,root);
+	}
+	else
+	{
+		leaves.push_back(root);
+	}
+}
+
+static bool						b3LeftOfAxis(	const b3DbvtNode* node,
+										   const b3Vector3& org,
+										   const b3Vector3& axis)
+{
+	return b3Dot(axis,node->volume.Center()-org) <= 0;
+}
+
+// Partitions leaves such that leaves[0, n) are on the
+// left of axis, and leaves[n, count) are on the right
+// of axis. returns N.
+static int						b3Split(	b3DbvtNode** leaves,
+									  int count,
+									  const b3Vector3& org,
+									  const b3Vector3& axis)
+{
+	int begin=0;
+	int end=count;
+	for(;;)
+	{
+		while(begin!=end && b3LeftOfAxis(leaves[begin],org,axis))
+		{
+			++begin;
+		}
+
+		if(begin==end)
+		{
+			break;
+		}
+
+		while(begin!=end && !b3LeftOfAxis(leaves[end-1],org,axis))
+		{
+			--end;
+		}
+
+		if(begin==end)
+		{
+			break;
+		}
+
+		// swap out of place nodes
+		--end;
+		b3DbvtNode* temp=leaves[begin];
+		leaves[begin]=leaves[end];
+		leaves[end]=temp;
+		++begin;
+	}
+
+	return begin;
+}
+
+//
+static b3DbvtVolume				b3Bounds(	b3DbvtNode** leaves,
+										 int count)
+{
+#if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE
+	B3_ATTRIBUTE_ALIGNED16(char	locals[sizeof(b3DbvtVolume)]);
+	b3DbvtVolume&	volume=*(b3DbvtVolume*)locals;
+	volume=leaves[0]->volume;
+#else
+	b3DbvtVolume volume=leaves[0]->volume;
+#endif
+	for(int i=1,ni=count;i<ni;++i)
+	{
+		b3Merge(volume,leaves[i]->volume,volume);
+	}
+	return(volume);
+}
+
+//
+static void						b3BottomUp(	b3DynamicBvh* pdbvt,
+										 b3DbvtNode** leaves,
+										 int count)
+{
+	while(count>1)
+	{
+		b3Scalar	minsize=B3_INFINITY;
+		int			minidx[2]={-1,-1};
+		for(int i=0;i<count;++i)
+		{
+			for(int j=i+1;j<count;++j)
+			{
+				const b3Scalar	sz=b3Size(b3Merge(leaves[i]->volume,leaves[j]->volume));
+				if(sz<minsize)
+				{
+					minsize		=	sz;
+					minidx[0]	=	i;
+					minidx[1]	=	j;
+				}
+			}
+		}
+		b3DbvtNode*	n[]	=	{leaves[minidx[0]],leaves[minidx[1]]};
+		b3DbvtNode*	p	=	b3CreateNode(pdbvt,0,n[0]->volume,n[1]->volume,0);
+		p->childs[0]		=	n[0];
+		p->childs[1]		=	n[1];
+		n[0]->parent		=	p;
+		n[1]->parent		=	p;
+		leaves[minidx[0]]	=	p;
+		leaves[minidx[1]]	=	leaves[count-1];
+		--count;
+	}
+}
+
+//
+static b3DbvtNode*			b3TopDown(b3DynamicBvh* pdbvt,
+									b3DbvtNode** leaves,
+									int count,
+									int bu_treshold)
+{
+	static const b3Vector3	axis[]={b3MakeVector3(1,0,0),
+		b3MakeVector3(0,1,0),
+		b3MakeVector3(0,0,1)};
+	b3Assert(bu_treshold>1);
+	if(count>1)
+	{
+		if(count>bu_treshold)
+		{
+			const b3DbvtVolume	vol=b3Bounds(leaves,count);
+			const b3Vector3			org=vol.Center();
+			int						partition;
+			int						bestaxis=-1;
+			int						bestmidp=count;
+			int						splitcount[3][2]={{0,0},{0,0},{0,0}};
+			int i;
+			for( i=0;i<count;++i)
+			{
+				const b3Vector3	x=leaves[i]->volume.Center()-org;
+				for(int j=0;j<3;++j)
+				{
+					++splitcount[j][b3Dot(x,axis[j])>0?1:0];
+				}
+			}
+			for( i=0;i<3;++i)
+			{
+				if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
+				{
+					const int	midp=(int)b3Fabs(b3Scalar(splitcount[i][0]-splitcount[i][1]));
+					if(midp<bestmidp)
+					{
+						bestaxis=i;
+						bestmidp=midp;
+					}
+				}
+			}
+			if(bestaxis>=0)
+			{
+				partition=b3Split(leaves,count,org,axis[bestaxis]);
+				b3Assert(partition!=0 && partition!=count);
+			}
+			else
+			{
+				partition=count/2+1;
+			}
+			b3DbvtNode*	node=b3CreateNode(pdbvt,0,vol,0);
+			node->childs[0]=b3TopDown(pdbvt,&leaves[0],partition,bu_treshold);
+			node->childs[1]=b3TopDown(pdbvt,&leaves[partition],count-partition,bu_treshold);
+			node->childs[0]->parent=node;
+			node->childs[1]->parent=node;
+			return(node);
+		}
+		else
+		{
+			b3BottomUp(pdbvt,leaves,count);
+			return(leaves[0]);
+		}
+	}
+	return(leaves[0]);
+}
+
+//
+static B3_DBVT_INLINE b3DbvtNode*	b3Sort(b3DbvtNode* n,b3DbvtNode*& r)
+{
+	b3DbvtNode*	p=n->parent;
+	b3Assert(n->isinternal());
+	if(p>n)
+	{
+		const int		i=b3IndexOf(n);
+		const int		j=1-i;
+		b3DbvtNode*	s=p->childs[j];
+		b3DbvtNode*	q=p->parent;
+		b3Assert(n==p->childs[i]);
+		if(q) q->childs[b3IndexOf(p)]=n; else r=n;
+		s->parent=n;
+		p->parent=n;
+		n->parent=q;
+		p->childs[0]=n->childs[0];
+		p->childs[1]=n->childs[1];
+		n->childs[0]->parent=p;
+		n->childs[1]->parent=p;
+		n->childs[i]=p;
+		n->childs[j]=s;
+		b3Swap(p->volume,n->volume);
+		return(p);
+	}
+	return(n);
+}
+
+#if 0
+static B3_DBVT_INLINE b3DbvtNode*	walkup(b3DbvtNode* n,int count)
+{
+	while(n&&(count--)) n=n->parent;
+	return(n);
+}
+#endif
+
+//
+// Api
+//
+
+//
+b3DynamicBvh::b3DynamicBvh()
+{
+	m_root		=	0;
+	m_free		=	0;
+	m_lkhd		=	-1;
+	m_leaves	=	0;
+	m_opath		=	0;
+}
+
+//
+b3DynamicBvh::~b3DynamicBvh()
+{
+	clear();
+}
+
+//
+void			b3DynamicBvh::clear()
+{
+	if(m_root)	
+		b3RecurseDeleteNode(this,m_root);
+	b3AlignedFree(m_free);
+	m_free=0;
+	m_lkhd		=	-1;
+	m_stkStack.clear();
+	m_opath		=	0;
+	
+}
+
+//
+void			b3DynamicBvh::optimizeBottomUp()
+{
+	if(m_root)
+	{
+		b3NodeArray leaves;
+		leaves.reserve(m_leaves);
+		b3FetchLeaves(this,m_root,leaves);
+		b3BottomUp(this,&leaves[0],leaves.size());
+		m_root=leaves[0];
+	}
+}
+
+//
+void			b3DynamicBvh::optimizeTopDown(int bu_treshold)
+{
+	if(m_root)
+	{
+		b3NodeArray	leaves;
+		leaves.reserve(m_leaves);
+		b3FetchLeaves(this,m_root,leaves);
+		m_root=b3TopDown(this,&leaves[0],leaves.size(),bu_treshold);
+	}
+}
+
+//
+void			b3DynamicBvh::optimizeIncremental(int passes)
+{
+	if(passes<0) passes=m_leaves;
+	if(m_root&&(passes>0))
+	{
+		do	{
+			b3DbvtNode*		node=m_root;
+			unsigned	bit=0;
+			while(node->isinternal())
+			{
+				node=b3Sort(node,m_root)->childs[(m_opath>>bit)&1];
+				bit=(bit+1)&(sizeof(unsigned)*8-1);
+			}
+			update(node);
+			++m_opath;
+		} while(--passes);
+	}
+}
+
+//
+b3DbvtNode*	b3DynamicBvh::insert(const b3DbvtVolume& volume,void* data)
+{
+	b3DbvtNode*	leaf=b3CreateNode(this,0,volume,data);
+	b3InsertLeaf(this,m_root,leaf);
+	++m_leaves;
+	return(leaf);
+}
+
+//
+void			b3DynamicBvh::update(b3DbvtNode* leaf,int lookahead)
+{
+	b3DbvtNode*	root=b3RemoveLeaf(this,leaf);
+	if(root)
+	{
+		if(lookahead>=0)
+		{
+			for(int i=0;(i<lookahead)&&root->parent;++i)
+			{
+				root=root->parent;
+			}
+		} else root=m_root;
+	}
+	b3InsertLeaf(this,root,leaf);
+}
+
+//
+void			b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume)
+{
+	b3DbvtNode*	root=b3RemoveLeaf(this,leaf);
+	if(root)
+	{
+		if(m_lkhd>=0)
+		{
+			for(int i=0;(i<m_lkhd)&&root->parent;++i)
+			{
+				root=root->parent;
+			}
+		} else root=m_root;
+	}
+	leaf->volume=volume;
+	b3InsertLeaf(this,root,leaf);
+}
+
+//
+bool			b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.Expand(b3MakeVector3(margin,margin,margin));
+	volume.SignedExpand(velocity);
+	update(leaf,volume);
+	return(true);
+}
+
+//
+bool			b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.SignedExpand(velocity);
+	update(leaf,volume);
+	return(true);
+}
+
+//
+bool			b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin)
+{
+	if(leaf->volume.Contain(volume)) return(false);
+	volume.Expand(b3MakeVector3(margin,margin,margin));
+	update(leaf,volume);
+	return(true);
+}
+
+//
+void			b3DynamicBvh::remove(b3DbvtNode* leaf)
+{
+	b3RemoveLeaf(this,leaf);
+	b3DeleteNode(this,leaf);
+	--m_leaves;
+}
+
+//
+void			b3DynamicBvh::write(IWriter* iwriter) const
+{
+	b3DbvtNodeEnumerator	nodes;
+	nodes.nodes.reserve(m_leaves*2);
+	enumNodes(m_root,nodes);
+	iwriter->Prepare(m_root,nodes.nodes.size());
+	for(int i=0;i<nodes.nodes.size();++i)
+	{
+		const b3DbvtNode* n=nodes.nodes[i];
+		int			p=-1;
+		if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
+		if(n->isinternal())
+		{
+			const int	c0=nodes.nodes.findLinearSearch(n->childs[0]);
+			const int	c1=nodes.nodes.findLinearSearch(n->childs[1]);
+			iwriter->WriteNode(n,i,p,c0,c1);
+		}
+		else
+		{
+			iwriter->WriteLeaf(n,i,p);
+		}	
+	}
+}
+
+//
+void			b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const
+{
+	dest.clear();
+	if(m_root!=0)
+	{	
+		b3AlignedObjectArray<sStkCLN>	stack;
+		stack.reserve(m_leaves);
+		stack.push_back(sStkCLN(m_root,0));
+		do	{
+			const int		i=stack.size()-1;
+			const sStkCLN	e=stack[i];
+			b3DbvtNode*			n=b3CreateNode(&dest,e.parent,e.node->volume,e.node->data);
+			stack.pop_back();
+			if(e.parent!=0)
+				e.parent->childs[i&1]=n;
+			else
+				dest.m_root=n;
+			if(e.node->isinternal())
+			{
+				stack.push_back(sStkCLN(e.node->childs[0],n));
+				stack.push_back(sStkCLN(e.node->childs[1],n));
+			}
+			else
+			{
+				iclone->CloneLeaf(n);
+			}
+		} while(stack.size()>0);
+	}
+}
+
+//
+int				b3DynamicBvh::maxdepth(const b3DbvtNode* node)
+{
+	int	depth=0;
+	if(node) b3GetMaxDepth(node,1,depth);
+	return(depth);
+}
+
+//
+int				b3DynamicBvh::countLeaves(const b3DbvtNode* node)
+{
+	if(node->isinternal())
+		return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
+	else
+		return(1);
+}
+
+//
+void			b3DynamicBvh::extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves)
+{
+	if(node->isinternal())
+	{
+		extractLeaves(node->childs[0],leaves);
+		extractLeaves(node->childs[1],leaves);
+	}
+	else
+	{
+		leaves.push_back(node);
+	}	
+}
+
+//
+#if B3_DBVT_ENABLE_BENCHMARK
+
+#include <stdio.h>
+#include <stdlib.h>
+
+
+/*
+q6600,2.4ghz
+
+/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
+/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
+/Fo"..\..\out\release8\build\libbulletcollision\\"
+/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
+/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
+
+Benchmarking dbvt...
+World scale: 100.000000
+Extents base: 1.000000
+Extents range: 4.000000
+Leaves: 8192
+sizeof(b3DbvtVolume): 32 bytes
+sizeof(b3DbvtNode):   44 bytes
+[1] b3DbvtVolume intersections: 3499 ms (-1%)
+[2] b3DbvtVolume merges: 1934 ms (0%)
+[3] b3DynamicBvh::collideTT: 5485 ms (-21%)
+[4] b3DynamicBvh::collideTT self: 2814 ms (-20%)
+[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%)
+[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%)
+[7] b3DynamicBvh::rayTest: 6314 ms (0%),(332143 r/s)
+[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
+[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
+[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
+[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
+[12] b3DbvtVolume notequal: 3659 ms (0%)
+[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
+[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
+[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
+[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
+[17] b3DbvtVolume select: 3419 ms (0%)
+*/
+
+struct b3DbvtBenchmark
+{
+	struct NilPolicy : b3DynamicBvh::ICollide
+	{
+		NilPolicy() : m_pcount(0),m_depth(-B3_INFINITY),m_checksort(true)		{}
+		void	Process(const b3DbvtNode*,const b3DbvtNode*)				{ ++m_pcount; }
+		void	Process(const b3DbvtNode*)									{ ++m_pcount; }
+		void	Process(const b3DbvtNode*,b3Scalar depth)
+		{
+			++m_pcount;
+			if(m_checksort)
+			{ if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
+		}
+		int			m_pcount;
+		b3Scalar	m_depth;
+		bool		m_checksort;
+	};
+	struct P14 : b3DynamicBvh::ICollide
+	{
+		struct Node
+		{
+			const b3DbvtNode*	leaf;
+			b3Scalar			depth;
+		};
+		void Process(const b3DbvtNode* leaf,b3Scalar depth)
+		{
+			Node	n;
+			n.leaf	=	leaf;
+			n.depth	=	depth;
+		}
+		static int sortfnc(const Node& a,const Node& b)
+		{
+			if(a.depth<b.depth) return(+1);
+			if(a.depth>b.depth) return(-1);
+			return(0);
+		}
+		b3AlignedObjectArray<Node>		m_nodes;
+	};
+	struct P15 : b3DynamicBvh::ICollide
+	{
+		struct Node
+		{
+			const b3DbvtNode*	leaf;
+			b3Scalar			depth;
+		};
+		void Process(const b3DbvtNode* leaf)
+		{
+			Node	n;
+			n.leaf	=	leaf;
+			n.depth	=	dot(leaf->volume.Center(),m_axis);
+		}
+		static int sortfnc(const Node& a,const Node& b)
+		{
+			if(a.depth<b.depth) return(+1);
+			if(a.depth>b.depth) return(-1);
+			return(0);
+		}
+		b3AlignedObjectArray<Node>		m_nodes;
+		b3Vector3						m_axis;
+	};
+	static b3Scalar			RandUnit()
+	{
+		return(rand()/(b3Scalar)RAND_MAX);
+	}
+	static b3Vector3		RandVector3()
+	{
+		return(b3Vector3(RandUnit(),RandUnit(),RandUnit()));
+	}
+	static b3Vector3		RandVector3(b3Scalar cs)
+	{
+		return(RandVector3()*cs-b3Vector3(cs,cs,cs)/2);
+	}
+	static b3DbvtVolume	RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es)
+	{
+		return(b3DbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es));
+	}
+	static b3Transform		RandTransform(b3Scalar cs)
+	{
+		b3Transform	t;
+		t.setOrigin(RandVector3(cs));
+		t.setRotation(b3Quaternion(RandUnit()*B3_PI*2,RandUnit()*B3_PI*2,RandUnit()*B3_PI*2).normalized());
+		return(t);
+	}
+	static void				RandTree(b3Scalar cs,b3Scalar eb,b3Scalar es,int leaves,b3DynamicBvh& dbvt)
+	{
+		dbvt.clear();
+		for(int i=0;i<leaves;++i)
+		{
+			dbvt.insert(RandVolume(cs,eb,es),0);
+		}
+	}
+};
+
+void			b3DynamicBvh::benchmark()
+{
+	static const b3Scalar	cfgVolumeCenterScale		=	100;
+	static const b3Scalar	cfgVolumeExentsBase			=	1;
+	static const b3Scalar	cfgVolumeExentsScale		=	4;
+	static const int		cfgLeaves					=	8192;
+	static const bool		cfgEnable					=	true;
+
+	//[1] b3DbvtVolume intersections
+	bool					cfgBenchmark1_Enable		=	cfgEnable;
+	static const int		cfgBenchmark1_Iterations	=	8;
+	static const int		cfgBenchmark1_Reference		=	3499;
+	//[2] b3DbvtVolume merges
+	bool					cfgBenchmark2_Enable		=	cfgEnable;
+	static const int		cfgBenchmark2_Iterations	=	4;
+	static const int		cfgBenchmark2_Reference		=	1945;
+	//[3] b3DynamicBvh::collideTT
+	bool					cfgBenchmark3_Enable		=	cfgEnable;
+	static const int		cfgBenchmark3_Iterations	=	512;
+	static const int		cfgBenchmark3_Reference		=	5485;
+	//[4] b3DynamicBvh::collideTT self
+	bool					cfgBenchmark4_Enable		=	cfgEnable;
+	static const int		cfgBenchmark4_Iterations	=	512;
+	static const int		cfgBenchmark4_Reference		=	2814;
+	//[5] b3DynamicBvh::collideTT xform
+	bool					cfgBenchmark5_Enable		=	cfgEnable;
+	static const int		cfgBenchmark5_Iterations	=	512;
+	static const b3Scalar	cfgBenchmark5_OffsetScale	=	2;
+	static const int		cfgBenchmark5_Reference		=	7379;
+	//[6] b3DynamicBvh::collideTT xform,self
+	bool					cfgBenchmark6_Enable		=	cfgEnable;
+	static const int		cfgBenchmark6_Iterations	=	512;
+	static const b3Scalar	cfgBenchmark6_OffsetScale	=	2;
+	static const int		cfgBenchmark6_Reference		=	7270;
+	//[7] b3DynamicBvh::rayTest
+	bool					cfgBenchmark7_Enable		=	cfgEnable;
+	static const int		cfgBenchmark7_Passes		=	32;
+	static const int		cfgBenchmark7_Iterations	=	65536;
+	static const int		cfgBenchmark7_Reference		=	6307;
+	//[8] insert/remove
+	bool					cfgBenchmark8_Enable		=	cfgEnable;
+	static const int		cfgBenchmark8_Passes		=	32;
+	static const int		cfgBenchmark8_Iterations	=	65536;
+	static const int		cfgBenchmark8_Reference		=	2105;
+	//[9] updates (teleport)
+	bool					cfgBenchmark9_Enable		=	cfgEnable;
+	static const int		cfgBenchmark9_Passes		=	32;
+	static const int		cfgBenchmark9_Iterations	=	65536;
+	static const int		cfgBenchmark9_Reference		=	1879;
+	//[10] updates (jitter)
+	bool					cfgBenchmark10_Enable		=	cfgEnable;
+	static const b3Scalar	cfgBenchmark10_Scale		=	cfgVolumeCenterScale/10000;
+	static const int		cfgBenchmark10_Passes		=	32;
+	static const int		cfgBenchmark10_Iterations	=	65536;
+	static const int		cfgBenchmark10_Reference	=	1244;
+	//[11] optimize (incremental)
+	bool					cfgBenchmark11_Enable		=	cfgEnable;
+	static const int		cfgBenchmark11_Passes		=	64;
+	static const int		cfgBenchmark11_Iterations	=	65536;
+	static const int		cfgBenchmark11_Reference	=	2510;
+	//[12] b3DbvtVolume notequal
+	bool					cfgBenchmark12_Enable		=	cfgEnable;
+	static const int		cfgBenchmark12_Iterations	=	32;
+	static const int		cfgBenchmark12_Reference	=	3677;
+	//[13] culling(OCL+fullsort)
+	bool					cfgBenchmark13_Enable		=	cfgEnable;
+	static const int		cfgBenchmark13_Iterations	=	1024;
+	static const int		cfgBenchmark13_Reference	=	2231;
+	//[14] culling(OCL+qsort)
+	bool					cfgBenchmark14_Enable		=	cfgEnable;
+	static const int		cfgBenchmark14_Iterations	=	8192;
+	static const int		cfgBenchmark14_Reference	=	3500;
+	//[15] culling(KDOP+qsort)
+	bool					cfgBenchmark15_Enable		=	cfgEnable;
+	static const int		cfgBenchmark15_Iterations	=	8192;
+	static const int		cfgBenchmark15_Reference	=	1151;
+	//[16] insert/remove batch
+	bool					cfgBenchmark16_Enable		=	cfgEnable;
+	static const int		cfgBenchmark16_BatchCount	=	256;
+	static const int		cfgBenchmark16_Passes		=	16384;
+	static const int		cfgBenchmark16_Reference	=	5138;
+	//[17] select
+	bool					cfgBenchmark17_Enable		=	cfgEnable;
+	static const int		cfgBenchmark17_Iterations	=	4;
+	static const int		cfgBenchmark17_Reference	=	3390;
+
+	b3Clock					wallclock;
+	printf("Benchmarking dbvt...\r\n");
+	printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
+	printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
+	printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
+	printf("\tLeaves: %u\r\n",cfgLeaves);
+	printf("\tsizeof(b3DbvtVolume): %u bytes\r\n",sizeof(b3DbvtVolume));
+	printf("\tsizeof(b3DbvtNode):   %u bytes\r\n",sizeof(b3DbvtNode));
+	if(cfgBenchmark1_Enable)
+	{// Benchmark 1	
+		srand(380843);
+		b3AlignedObjectArray<b3DbvtVolume>	volumes;
+		b3AlignedObjectArray<bool>			results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[1] b3DbvtVolume intersections: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark1_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					results[k]=Intersect(volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
+	}
+	if(cfgBenchmark2_Enable)
+	{// Benchmark 2	
+		srand(380843);
+		b3AlignedObjectArray<b3DbvtVolume>	volumes;
+		b3AlignedObjectArray<b3DbvtVolume>	results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[2] b3DbvtVolume merges: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark2_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					Merge(volumes[j],volumes[k],results[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
+	}
+	if(cfgBenchmark3_Enable)
+	{// Benchmark 3	
+		srand(380843);
+		b3DynamicBvh						dbvt[2];
+		b3DbvtBenchmark::NilPolicy	policy;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[3] b3DynamicBvh::collideTT: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark3_Iterations;++i)
+		{
+			b3DynamicBvh::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
+	}
+	if(cfgBenchmark4_Enable)
+	{// Benchmark 4
+		srand(380843);
+		b3DynamicBvh						dbvt;
+		b3DbvtBenchmark::NilPolicy	policy;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[4] b3DynamicBvh::collideTT self: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark4_Iterations;++i)
+		{
+			b3DynamicBvh::collideTT(dbvt.m_root,dbvt.m_root,policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
+	}
+	if(cfgBenchmark5_Enable)
+	{// Benchmark 5	
+		srand(380843);
+		b3DynamicBvh								dbvt[2];
+		b3AlignedObjectArray<b3Transform>	transforms;
+		b3DbvtBenchmark::NilPolicy			policy;
+		transforms.resize(cfgBenchmark5_Iterations);
+		for(int i=0;i<transforms.size();++i)
+		{
+			transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
+		dbvt[0].optimizeTopDown();
+		dbvt[1].optimizeTopDown();
+		printf("[5] b3DynamicBvh::collideTT xform: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark5_Iterations;++i)
+		{
+			b3DynamicBvh::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
+	}
+	if(cfgBenchmark6_Enable)
+	{// Benchmark 6	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3Transform>	transforms;
+		b3DbvtBenchmark::NilPolicy			policy;
+		transforms.resize(cfgBenchmark6_Iterations);
+		for(int i=0;i<transforms.size();++i)
+		{
+			transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[6] b3DynamicBvh::collideTT xform,self: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark6_Iterations;++i)
+		{
+			b3DynamicBvh::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);		
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
+	}
+	if(cfgBenchmark7_Enable)
+	{// Benchmark 7	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3Vector3>		rayorg;
+		b3AlignedObjectArray<b3Vector3>		raydir;
+		b3DbvtBenchmark::NilPolicy			policy;
+		rayorg.resize(cfgBenchmark7_Iterations);
+		raydir.resize(cfgBenchmark7_Iterations);
+		for(int i=0;i<rayorg.size();++i)
+		{
+			rayorg[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+			raydir[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[7] b3DynamicBvh::rayTest: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark7_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark7_Iterations;++j)
+			{
+				b3DynamicBvh::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		unsigned	rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
+		printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
+	}
+	if(cfgBenchmark8_Enable)
+	{// Benchmark 8	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[8] insert/remove: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark8_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark8_Iterations;++j)
+			{
+				dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
+		printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
+	}
+	if(cfgBenchmark9_Enable)
+	{// Benchmark 9	
+		srand(380843);
+		b3DynamicBvh										dbvt;
+		b3AlignedObjectArray<const b3DbvtNode*>	leaves;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		dbvt.extractLeaves(dbvt.m_root,leaves);
+		printf("[9] updates (teleport): ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark9_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark9_Iterations;++j)
+			{
+				dbvt.update(const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]),
+					b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
+	}
+	if(cfgBenchmark10_Enable)
+	{// Benchmark 10	
+		srand(380843);
+		b3DynamicBvh										dbvt;
+		b3AlignedObjectArray<const b3DbvtNode*>	leaves;
+		b3AlignedObjectArray<b3Vector3>				vectors;
+		vectors.resize(cfgBenchmark10_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1))*cfgBenchmark10_Scale;
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		dbvt.extractLeaves(dbvt.m_root,leaves);
+		printf("[10] updates (jitter): ");
+		wallclock.reset();
+
+		for(int i=0;i<cfgBenchmark10_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark10_Iterations;++j)
+			{			
+				const b3Vector3&	d=vectors[j];
+				b3DbvtNode*		l=const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]);
+				b3DbvtVolume		v=b3DbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
+				dbvt.update(l,v);
+			}
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
+		printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
+	}
+	if(cfgBenchmark11_Enable)
+	{// Benchmark 11	
+		srand(380843);
+		b3DynamicBvh										dbvt;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[11] optimize (incremental): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark11_Passes;++i)
+		{
+			dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
+		printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
+	}
+	if(cfgBenchmark12_Enable)
+	{// Benchmark 12	
+		srand(380843);
+		b3AlignedObjectArray<b3DbvtVolume>	volumes;
+		b3AlignedObjectArray<bool>				results;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		printf("[12] b3DbvtVolume notequal: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark12_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					results[k]=NotEqual(volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
+	}
+	if(cfgBenchmark13_Enable)
+	{// Benchmark 13	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3Vector3>		vectors;
+		b3DbvtBenchmark::NilPolicy			policy;
+		vectors.resize(cfgBenchmark13_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		printf("[13] culling(OCL+fullsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark13_Iterations;++i)
+		{
+			static const b3Scalar	offset=0;
+			policy.m_depth=-B3_INFINITY;
+			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark13_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark14_Enable)
+	{// Benchmark 14	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3Vector3>		vectors;
+		b3DbvtBenchmark::P14				policy;
+		vectors.resize(cfgBenchmark14_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		policy.m_nodes.reserve(cfgLeaves);
+		printf("[14] culling(OCL+qsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark14_Iterations;++i)
+		{
+			static const b3Scalar	offset=0;
+			policy.m_nodes.resize(0);
+			dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
+			policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark14_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark15_Enable)
+	{// Benchmark 15	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3Vector3>		vectors;
+		b3DbvtBenchmark::P15				policy;
+		vectors.resize(cfgBenchmark15_Iterations);
+		for(int i=0;i<vectors.size();++i)
+		{
+			vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
+		}
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		policy.m_nodes.reserve(cfgLeaves);
+		printf("[15] culling(KDOP+qsort): ");
+		wallclock.reset();	
+		for(int i=0;i<cfgBenchmark15_Iterations;++i)
+		{
+			static const b3Scalar	offset=0;
+			policy.m_nodes.resize(0);
+			policy.m_axis=vectors[i];
+			dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
+			policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	t=cfgBenchmark15_Iterations;
+		printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
+	}
+	if(cfgBenchmark16_Enable)
+	{// Benchmark 16	
+		srand(380843);
+		b3DynamicBvh								dbvt;
+		b3AlignedObjectArray<b3DbvtNode*>	batch;
+		b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
+		dbvt.optimizeTopDown();
+		batch.reserve(cfgBenchmark16_BatchCount);
+		printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark16_Passes;++i)
+		{
+			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			{
+				batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
+			}
+			for(int j=0;j<cfgBenchmark16_BatchCount;++j)
+			{
+				dbvt.remove(batch[j]);
+			}
+			batch.resize(0);
+		}
+		const int	time=(int)wallclock.getTimeMilliseconds();
+		const int	ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
+		printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
+	}
+	if(cfgBenchmark17_Enable)
+	{// Benchmark 17
+		srand(380843);
+		b3AlignedObjectArray<b3DbvtVolume>	volumes;
+		b3AlignedObjectArray<int>			results;
+		b3AlignedObjectArray<int>			indices;
+		volumes.resize(cfgLeaves);
+		results.resize(cfgLeaves);
+		indices.resize(cfgLeaves);
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			indices[i]=i;
+			volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
+		}
+		for(int i=0;i<cfgLeaves;++i)
+		{
+			b3Swap(indices[i],indices[rand()%cfgLeaves]);
+		}
+		printf("[17] b3DbvtVolume select: ");
+		wallclock.reset();
+		for(int i=0;i<cfgBenchmark17_Iterations;++i)
+		{
+			for(int j=0;j<cfgLeaves;++j)
+			{
+				for(int k=0;k<cfgLeaves;++k)
+				{
+					const int idx=indices[k];
+					results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
+				}
+			}
+		}
+		const int time=(int)wallclock.getTimeMilliseconds();
+		printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
+	}
+	printf("\r\n\r\n");
+}
+#endif

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h

@@ -0,0 +1,1269 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///b3DynamicBvh implementation by Nathanael Presson
+
+#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
+#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+
+//
+// Compile time configuration
+//
+
+
+// Implementation profiles
+#define B3_DBVT_IMPL_GENERIC		0	// Generic implementation	
+#define B3_DBVT_IMPL_SSE			1	// SSE
+
+// Template implementation of ICollide
+#ifdef _WIN32
+#if (defined (_MSC_VER) && _MSC_VER >= 1400)
+#define	B3_DBVT_USE_TEMPLATE		1
+#else
+#define	B3_DBVT_USE_TEMPLATE		0
+#endif
+#else
+#define	B3_DBVT_USE_TEMPLATE		0
+#endif
+
+// Use only intrinsics instead of inline asm
+#define B3_DBVT_USE_INTRINSIC_SSE	1
+
+// Using memmov for collideOCL
+#define B3_DBVT_USE_MEMMOVE		1
+
+// Enable benchmarking code
+#define	B3_DBVT_ENABLE_BENCHMARK	0
+
+// Inlining
+#define B3_DBVT_INLINE				B3_FORCE_INLINE
+
+// Specific methods implementation
+
+//SSE gives errors on a MSVC 7.1
+#if defined (B3_USE_SSE) //&& defined (_WIN32)
+#define B3_DBVT_SELECT_IMPL		B3_DBVT_IMPL_SSE
+#define B3_DBVT_MERGE_IMPL			B3_DBVT_IMPL_SSE
+#define B3_DBVT_INT0_IMPL			B3_DBVT_IMPL_SSE
+#else
+#define B3_DBVT_SELECT_IMPL		B3_DBVT_IMPL_GENERIC
+#define B3_DBVT_MERGE_IMPL			B3_DBVT_IMPL_GENERIC
+#define B3_DBVT_INT0_IMPL			B3_DBVT_IMPL_GENERIC
+#endif
+
+#if	(B3_DBVT_SELECT_IMPL==B3_DBVT_IMPL_SSE)||	\
+	(B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE)||	\
+	(B3_DBVT_INT0_IMPL==B3_DBVT_IMPL_SSE)
+#include <emmintrin.h>
+#endif
+
+//
+// Auto config and checks
+//
+
+#if B3_DBVT_USE_TEMPLATE
+#define	B3_DBVT_VIRTUAL
+#define B3_DBVT_VIRTUAL_DTOR(a)
+#define B3_DBVT_PREFIX					template <typename T>
+#define B3_DBVT_IPOLICY				T& policy
+#define B3_DBVT_CHECKTYPE				static const ICollide&	typechecker=*(T*)1;(void)typechecker;
+#else
+#define	B3_DBVT_VIRTUAL_DTOR(a)		virtual ~a() {}
+#define B3_DBVT_VIRTUAL				virtual
+#define B3_DBVT_PREFIX
+#define B3_DBVT_IPOLICY				ICollide& policy
+#define B3_DBVT_CHECKTYPE
+#endif
+
+#if B3_DBVT_USE_MEMMOVE
+#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
+#include <memory.h>
+#endif
+#include <string.h>
+#endif
+
+#ifndef B3_DBVT_USE_TEMPLATE
+#error "B3_DBVT_USE_TEMPLATE undefined"
+#endif
+
+#ifndef B3_DBVT_USE_MEMMOVE
+#error "B3_DBVT_USE_MEMMOVE undefined"
+#endif
+
+#ifndef B3_DBVT_ENABLE_BENCHMARK
+#error "B3_DBVT_ENABLE_BENCHMARK undefined"
+#endif
+
+#ifndef B3_DBVT_SELECT_IMPL
+#error "B3_DBVT_SELECT_IMPL undefined"
+#endif
+
+#ifndef B3_DBVT_MERGE_IMPL
+#error "B3_DBVT_MERGE_IMPL undefined"
+#endif
+
+#ifndef B3_DBVT_INT0_IMPL
+#error "B3_DBVT_INT0_IMPL undefined"
+#endif
+
+//
+// Defaults volumes
+//
+
+/* b3DbvtAabbMm			*/ 
+struct	b3DbvtAabbMm
+{
+	B3_DBVT_INLINE b3Vector3			Center() const	{ return((mi+mx)/2); }
+	B3_DBVT_INLINE b3Vector3			Lengths() const	{ return(mx-mi); }
+	B3_DBVT_INLINE b3Vector3			Extents() const	{ return((mx-mi)/2); }
+	B3_DBVT_INLINE const b3Vector3&	Mins() const	{ return(mi); }
+	B3_DBVT_INLINE const b3Vector3&	Maxs() const	{ return(mx); }
+	static inline b3DbvtAabbMm		FromCE(const b3Vector3& c,const b3Vector3& e);
+	static inline b3DbvtAabbMm		FromCR(const b3Vector3& c,b3Scalar r);
+	static inline b3DbvtAabbMm		FromMM(const b3Vector3& mi,const b3Vector3& mx);
+	static inline b3DbvtAabbMm		FromPoints(const b3Vector3* pts,int n);
+	static inline b3DbvtAabbMm		FromPoints(const b3Vector3** ppts,int n);
+	B3_DBVT_INLINE void				Expand(const b3Vector3& e);
+	B3_DBVT_INLINE void				SignedExpand(const b3Vector3& e);
+	B3_DBVT_INLINE bool				Contain(const b3DbvtAabbMm& a) const;
+	B3_DBVT_INLINE int					Classify(const b3Vector3& n,b3Scalar o,int s) const;
+	B3_DBVT_INLINE b3Scalar			ProjectMinimum(const b3Vector3& v,unsigned signs) const;
+	B3_DBVT_INLINE friend bool			b3Intersect(	const b3DbvtAabbMm& a,
+		const b3DbvtAabbMm& b);
+	
+	B3_DBVT_INLINE friend bool			b3Intersect(	const b3DbvtAabbMm& a,
+		const b3Vector3& b);
+
+	B3_DBVT_INLINE friend b3Scalar		b3Proximity(	const b3DbvtAabbMm& a,
+		const b3DbvtAabbMm& b);
+	B3_DBVT_INLINE friend int			b3Select(		const b3DbvtAabbMm& o,
+		const b3DbvtAabbMm& a,
+		const b3DbvtAabbMm& b);
+	B3_DBVT_INLINE friend void			b3Merge(		const b3DbvtAabbMm& a,
+		const b3DbvtAabbMm& b,
+		b3DbvtAabbMm& r);
+	B3_DBVT_INLINE friend bool			b3NotEqual(	const b3DbvtAabbMm& a,
+		const b3DbvtAabbMm& b);
+    
+    B3_DBVT_INLINE b3Vector3&	tMins()	{ return(mi); }
+	B3_DBVT_INLINE b3Vector3&	tMaxs()	{ return(mx); }
+    
+private:
+	B3_DBVT_INLINE void				AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const;
+private:
+	b3Vector3	mi,mx;
+};
+
+// Types	
+typedef	b3DbvtAabbMm	b3DbvtVolume;
+
+/* b3DbvtNode				*/ 
+struct	b3DbvtNode
+{
+	b3DbvtVolume	volume;
+	b3DbvtNode*		parent;
+	B3_DBVT_INLINE bool	isleaf() const		{ return(childs[1]==0); }
+	B3_DBVT_INLINE bool	isinternal() const	{ return(!isleaf()); }
+	union
+	{
+		b3DbvtNode*	childs[2];
+		void*	data;
+		int		dataAsInt;
+	};
+};
+
+///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
+///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes.
+///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
+struct	b3DynamicBvh
+{
+	/* Stack element	*/ 
+	struct	sStkNN
+	{
+		const b3DbvtNode*	a;
+		const b3DbvtNode*	b;
+		sStkNN() {}
+		sStkNN(const b3DbvtNode* na,const b3DbvtNode* nb) : a(na),b(nb) {}
+	};
+	struct	sStkNP
+	{
+		const b3DbvtNode*	node;
+		int			mask;
+		sStkNP(const b3DbvtNode* n,unsigned m) : node(n),mask(m) {}
+	};
+	struct	sStkNPS
+	{
+		const b3DbvtNode*	node;
+		int			mask;
+		b3Scalar	value;
+		sStkNPS() {}
+		sStkNPS(const b3DbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {}
+	};
+	struct	sStkCLN
+	{
+		const b3DbvtNode*	node;
+		b3DbvtNode*		parent;
+		sStkCLN(const b3DbvtNode* n,b3DbvtNode* p) : node(n),parent(p) {}
+	};
+	// Policies/Interfaces
+
+	/* ICollide	*/ 
+	struct	ICollide
+	{		
+		B3_DBVT_VIRTUAL_DTOR(ICollide)
+			B3_DBVT_VIRTUAL void	Process(const b3DbvtNode*,const b3DbvtNode*)		{}
+		B3_DBVT_VIRTUAL void	Process(const b3DbvtNode*)					{}
+		B3_DBVT_VIRTUAL void	Process(const b3DbvtNode* n,b3Scalar)			{ Process(n); }
+		B3_DBVT_VIRTUAL bool	Descent(const b3DbvtNode*)					{ return(true); }
+		B3_DBVT_VIRTUAL bool	AllLeaves(const b3DbvtNode*)					{ return(true); }
+	};
+	/* IWriter	*/ 
+	struct	IWriter
+	{
+		virtual ~IWriter() {}
+		virtual void		Prepare(const b3DbvtNode* root,int numnodes)=0;
+		virtual void		WriteNode(const b3DbvtNode*,int index,int parent,int child0,int child1)=0;
+		virtual void		WriteLeaf(const b3DbvtNode*,int index,int parent)=0;
+	};
+	/* IClone	*/ 
+	struct	IClone
+	{
+		virtual ~IClone()	{}
+		virtual void		CloneLeaf(b3DbvtNode*) {}
+	};
+
+	// Constants
+	enum	{
+		B3_SIMPLE_STACKSIZE	=	64,
+		B3_DOUBLE_STACKSIZE	=	B3_SIMPLE_STACKSIZE*2
+	};
+
+	// Fields
+	b3DbvtNode*		m_root;
+	b3DbvtNode*		m_free;
+	int				m_lkhd;
+	int				m_leaves;
+	unsigned		m_opath;
+
+	
+	b3AlignedObjectArray<sStkNN>	m_stkStack;
+	mutable b3AlignedObjectArray<const b3DbvtNode*>	m_rayTestStack;
+
+
+	// Methods
+	b3DynamicBvh();
+	~b3DynamicBvh();
+	void			clear();
+	bool			empty() const { return(0==m_root); }
+	void			optimizeBottomUp();
+	void			optimizeTopDown(int bu_treshold=128);
+	void			optimizeIncremental(int passes);
+	b3DbvtNode*		insert(const b3DbvtVolume& box,void* data);
+	void			update(b3DbvtNode* leaf,int lookahead=-1);
+	void			update(b3DbvtNode* leaf,b3DbvtVolume& volume);
+	bool			update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin);
+	bool			update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity);
+	bool			update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin);	
+	void			remove(b3DbvtNode* leaf);
+	void			write(IWriter* iwriter) const;
+	void			clone(b3DynamicBvh& dest,IClone* iclone=0) const;
+	static int		maxdepth(const b3DbvtNode* node);
+	static int		countLeaves(const b3DbvtNode* node);
+	static void		extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves);
+#if B3_DBVT_ENABLE_BENCHMARK
+	static void		benchmark();
+#else
+	static void		benchmark(){}
+#endif
+	// B3_DBVT_IPOLICY must support ICollide policy/interface
+	B3_DBVT_PREFIX
+		static void		enumNodes(	const b3DbvtNode* root,
+		B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+		static void		enumLeaves(	const b3DbvtNode* root,
+		B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+		void		collideTT(	const b3DbvtNode* root0,
+		const b3DbvtNode* root1,
+		B3_DBVT_IPOLICY);
+
+	B3_DBVT_PREFIX
+		void		collideTTpersistentStack(	const b3DbvtNode* root0,
+		  const b3DbvtNode* root1,
+		  B3_DBVT_IPOLICY);
+#if 0
+	B3_DBVT_PREFIX
+		void		collideTT(	const b3DbvtNode* root0,
+		const b3DbvtNode* root1,
+		const b3Transform& xform,
+		B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+		void		collideTT(	const b3DbvtNode* root0,
+		const b3Transform& xform0,
+		const b3DbvtNode* root1,
+		const b3Transform& xform1,
+		B3_DBVT_IPOLICY);
+#endif
+
+	B3_DBVT_PREFIX
+		void		collideTV(	const b3DbvtNode* root,
+		const b3DbvtVolume& volume,
+		B3_DBVT_IPOLICY) const;
+	///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc is thread-safe (uses locking etc)
+	///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
+	B3_DBVT_PREFIX
+		static void		rayTest(	const b3DbvtNode* root,
+		const b3Vector3& rayFrom,
+		const b3Vector3& rayTo,
+		B3_DBVT_IPOLICY);
+	///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
+	///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts
+	B3_DBVT_PREFIX
+		void		rayTestInternal(	const b3DbvtNode* root,
+								const b3Vector3& rayFrom,
+								const b3Vector3& rayTo,
+								const b3Vector3& rayDirectionInverse,
+								unsigned int signs[3],
+								b3Scalar lambda_max,
+								const b3Vector3& aabbMin,
+								const b3Vector3& aabbMax,
+								B3_DBVT_IPOLICY) const;
+
+	B3_DBVT_PREFIX
+		static void		collideKDOP(const b3DbvtNode* root,
+		const b3Vector3* normals,
+		const b3Scalar* offsets,
+		int count,
+		B3_DBVT_IPOLICY);
+	B3_DBVT_PREFIX
+		static void		collideOCL(	const b3DbvtNode* root,
+		const b3Vector3* normals,
+		const b3Scalar* offsets,
+		const b3Vector3& sortaxis,
+		int count,								
+		B3_DBVT_IPOLICY,
+		bool fullsort=true);
+	B3_DBVT_PREFIX
+		static void		collideTU(	const b3DbvtNode* root,
+		B3_DBVT_IPOLICY);
+	// Helpers	
+	static B3_DBVT_INLINE int	nearest(const int* i,const b3DynamicBvh::sStkNPS* a,b3Scalar v,int l,int h)
+	{
+		int	m=0;
+		while(l<h)
+		{
+			m=(l+h)>>1;
+			if(a[i[m]].value>=v) l=m+1; else h=m;
+		}
+		return(h);
+	}
+	static B3_DBVT_INLINE int	allocate(	b3AlignedObjectArray<int>& ifree,
+		b3AlignedObjectArray<sStkNPS>& stock,
+		const sStkNPS& value)
+	{
+		int	i;
+		if(ifree.size()>0)
+		{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
+		else
+		{ i=stock.size();stock.push_back(value); }
+		return(i); 
+	}
+	//
+private:
+	b3DynamicBvh(const b3DynamicBvh&)	{}	
+};
+
+//
+// Inline's
+//
+
+//
+inline b3DbvtAabbMm			b3DbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e)
+{
+	b3DbvtAabbMm box;
+	box.mi=c-e;box.mx=c+e;
+	return(box);
+}
+
+//
+inline b3DbvtAabbMm			b3DbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r)
+{
+	return(FromCE(c,b3MakeVector3(r,r,r)));
+}
+
+//
+inline b3DbvtAabbMm			b3DbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx)
+{
+	b3DbvtAabbMm box;
+	box.mi=mi;box.mx=mx;
+	return(box);
+}
+
+//
+inline b3DbvtAabbMm			b3DbvtAabbMm::FromPoints(const b3Vector3* pts,int n)
+{
+	b3DbvtAabbMm box;
+	box.mi=box.mx=pts[0];
+	for(int i=1;i<n;++i)
+	{
+		box.mi.setMin(pts[i]);
+		box.mx.setMax(pts[i]);
+	}
+	return(box);
+}
+
+//
+inline b3DbvtAabbMm			b3DbvtAabbMm::FromPoints(const b3Vector3** ppts,int n)
+{
+	b3DbvtAabbMm box;
+	box.mi=box.mx=*ppts[0];
+	for(int i=1;i<n;++i)
+	{
+		box.mi.setMin(*ppts[i]);
+		box.mx.setMax(*ppts[i]);
+	}
+	return(box);
+}
+
+//
+B3_DBVT_INLINE void		b3DbvtAabbMm::Expand(const b3Vector3& e)
+{
+	mi-=e;mx+=e;
+}
+
+//
+B3_DBVT_INLINE void		b3DbvtAabbMm::SignedExpand(const b3Vector3& e)
+{
+	if(e.x>0) mx.setX(mx.x+e[0]); else mi.setX(mi.x+e[0]);
+	if(e.y>0) mx.setY(mx.y+e[1]); else mi.setY(mi.y+e[1]);
+	if(e.z>0) mx.setZ(mx.z+e[2]); else mi.setZ(mi.z+e[2]);
+}
+
+//
+B3_DBVT_INLINE bool		b3DbvtAabbMm::Contain(const b3DbvtAabbMm& a) const
+{
+	return(	(mi.x<=a.mi.x)&&
+		(mi.y<=a.mi.y)&&
+		(mi.z<=a.mi.z)&&
+		(mx.x>=a.mx.x)&&
+		(mx.y>=a.mx.y)&&
+		(mx.z>=a.mx.z));
+}
+
+//
+B3_DBVT_INLINE int		b3DbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const
+{
+	b3Vector3			pi,px;
+	switch(s)
+	{
+	case	(0+0+0):	px=b3MakeVector3(mi.x,mi.y,mi.z);
+		pi=b3MakeVector3(mx.x,mx.y,mx.z);break;
+	case	(1+0+0):	px=b3MakeVector3(mx.x,mi.y,mi.z);
+		pi=b3MakeVector3(mi.x,mx.y,mx.z);break;
+	case	(0+2+0):	px=b3MakeVector3(mi.x,mx.y,mi.z);
+		pi=b3MakeVector3(mx.x,mi.y,mx.z);break;
+	case	(1+2+0):	px=b3MakeVector3(mx.x,mx.y,mi.z);
+		pi=b3MakeVector3(mi.x,mi.y,mx.z);break;
+	case	(0+0+4):	px=b3MakeVector3(mi.x,mi.y,mx.z);
+		pi=b3MakeVector3(mx.x,mx.y,mi.z);break;
+	case	(1+0+4):	px=b3MakeVector3(mx.x,mi.y,mx.z);
+		pi=b3MakeVector3(mi.x,mx.y,mi.z);break;
+	case	(0+2+4):	px=b3MakeVector3(mi.x,mx.y,mx.z);
+		pi=b3MakeVector3(mx.x,mi.y,mi.z);break;
+	case	(1+2+4):	px=b3MakeVector3(mx.x,mx.y,mx.z);
+		pi=b3MakeVector3(mi.x,mi.y,mi.z);break;
+	}
+	if((b3Dot(n,px)+o)<0)		return(-1);
+	if((b3Dot(n,pi)+o)>=0)	return(+1);
+	return(0);
+}
+
+//
+B3_DBVT_INLINE b3Scalar	b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const
+{
+	const b3Vector3*	b[]={&mx,&mi};
+	const b3Vector3		p = b3MakeVector3(	b[(signs>>0)&1]->x,
+		b[(signs>>1)&1]->y,
+		b[(signs>>2)&1]->z);
+	return(b3Dot(p,v));
+}
+
+//
+B3_DBVT_INLINE void		b3DbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const
+{
+	for(int i=0;i<3;++i)
+	{
+		if(d[i]<0)
+		{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
+		else
+		{ smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
+	}
+}
+
+//
+B3_DBVT_INLINE bool		b3Intersect(	const b3DbvtAabbMm& a,
+								  const b3DbvtAabbMm& b)
+{
+#if	B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE
+	const __m128	rt(_mm_or_ps(	_mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
+		_mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
+#if defined (_WIN32)
+	const __int32*	pu((const __int32*)&rt);
+#else
+    const int*	pu((const int*)&rt);
+#endif
+	return((pu[0]|pu[1]|pu[2])==0);
+#else
+	return(	(a.mi.x<=b.mx.x)&&
+		(a.mx.x>=b.mi.x)&&
+		(a.mi.y<=b.mx.y)&&
+		(a.mx.y>=b.mi.y)&&
+		(a.mi.z<=b.mx.z)&&		
+		(a.mx.z>=b.mi.z));
+#endif
+}
+
+
+
+//
+B3_DBVT_INLINE bool		b3Intersect(	const b3DbvtAabbMm& a,
+								  const b3Vector3& b)
+{
+	return(	(b.x>=a.mi.x)&&
+		(b.y>=a.mi.y)&&
+		(b.z>=a.mi.z)&&
+		(b.x<=a.mx.x)&&
+		(b.y<=a.mx.y)&&
+		(b.z<=a.mx.z));
+}
+
+
+
+
+
+//////////////////////////////////////
+
+
+//
+B3_DBVT_INLINE b3Scalar	b3Proximity(	const b3DbvtAabbMm& a,
+								  const b3DbvtAabbMm& b)
+{
+	const b3Vector3	d=(a.mi+a.mx)-(b.mi+b.mx);
+	return(b3Fabs(d.x)+b3Fabs(d.y)+b3Fabs(d.z));
+}
+
+
+
+//
+B3_DBVT_INLINE int			b3Select(	const b3DbvtAabbMm& o,
+							   const b3DbvtAabbMm& a,
+							   const b3DbvtAabbMm& b)
+{
+#if	B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE
+    
+#if defined (_WIN32)
+	static B3_ATTRIBUTE_ALIGNED16(const unsigned __int32)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
+#else
+    static B3_ATTRIBUTE_ALIGNED16(const unsigned int)	mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/};
+#endif
+	///@todo: the intrinsic version is 11% slower
+#if B3_DBVT_USE_INTRINSIC_SSE
+
+	union b3SSEUnion ///NOTE: if we use more intrinsics, move b3SSEUnion into the LinearMath directory
+	{
+	   __m128		ssereg;
+	   float		floats[4];
+	   int			ints[4];
+	};
+
+	__m128	omi(_mm_load_ps(o.mi));
+	omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
+	__m128	ami(_mm_load_ps(a.mi));
+	ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
+	ami=_mm_sub_ps(ami,omi);
+	ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
+	__m128	bmi(_mm_load_ps(b.mi));
+	bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
+	bmi=_mm_sub_ps(bmi,omi);
+	bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
+	__m128	t0(_mm_movehl_ps(ami,ami));
+	ami=_mm_add_ps(ami,t0);
+	ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
+	__m128 t1(_mm_movehl_ps(bmi,bmi));
+	bmi=_mm_add_ps(bmi,t1);
+	bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
+	
+	b3SSEUnion tmp;
+	tmp.ssereg = _mm_cmple_ss(bmi,ami);
+	return tmp.ints[0]&1;
+
+#else
+	B3_ATTRIBUTE_ALIGNED16(__int32	r[1]);
+	__asm
+	{
+		mov		eax,o
+			mov		ecx,a
+			mov		edx,b
+			movaps	xmm0,[eax]
+		movaps	xmm5,mask
+			addps	xmm0,[eax+16]	
+		movaps	xmm1,[ecx]
+		movaps	xmm2,[edx]
+		addps	xmm1,[ecx+16]
+		addps	xmm2,[edx+16]
+		subps	xmm1,xmm0
+			subps	xmm2,xmm0
+			andps	xmm1,xmm5
+			andps	xmm2,xmm5
+			movhlps	xmm3,xmm1
+			movhlps	xmm4,xmm2
+			addps	xmm1,xmm3
+			addps	xmm2,xmm4
+			pshufd	xmm3,xmm1,1
+			pshufd	xmm4,xmm2,1
+			addss	xmm1,xmm3
+			addss	xmm2,xmm4
+			cmpless	xmm2,xmm1
+			movss	r,xmm2
+	}
+	return(r[0]&1);
+#endif
+#else
+	return(b3Proximity(o,a)<b3Proximity(o,b)?0:1);
+#endif
+}
+
+//
+B3_DBVT_INLINE void		b3Merge(	const b3DbvtAabbMm& a,
+							  const b3DbvtAabbMm& b,
+							  b3DbvtAabbMm& r)
+{
+#if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE
+	__m128	ami(_mm_load_ps(a.mi));
+	__m128	amx(_mm_load_ps(a.mx));
+	__m128	bmi(_mm_load_ps(b.mi));
+	__m128	bmx(_mm_load_ps(b.mx));
+	ami=_mm_min_ps(ami,bmi);
+	amx=_mm_max_ps(amx,bmx);
+	_mm_store_ps(r.mi,ami);
+	_mm_store_ps(r.mx,amx);
+#else
+	for(int i=0;i<3;++i)
+	{
+		if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
+		if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
+	}
+#endif
+}
+
+//
+B3_DBVT_INLINE bool		b3NotEqual(	const b3DbvtAabbMm& a,
+								 const b3DbvtAabbMm& b)
+{
+	return(	(a.mi.x!=b.mi.x)||
+		(a.mi.y!=b.mi.y)||
+		(a.mi.z!=b.mi.z)||
+		(a.mx.x!=b.mx.x)||
+		(a.mx.y!=b.mx.y)||
+		(a.mx.z!=b.mx.z));
+}
+
+//
+// Inline's
+//
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::enumNodes(	const b3DbvtNode* root,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		policy.Process(root);
+	if(root->isinternal())
+	{
+		enumNodes(root->childs[0],policy);
+		enumNodes(root->childs[1],policy);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::enumLeaves(	const b3DbvtNode* root,
+								   B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root->isinternal())
+		{
+			enumLeaves(root->childs[0],policy);
+			enumLeaves(root->childs[1],policy);
+		}
+		else
+		{
+			policy.Process(root);
+		}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
+								  const b3DbvtNode* root1,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=B3_DOUBLE_STACKSIZE-4;
+			b3AlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(B3_DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=stkStack[--depth];
+				if(depth>treshold)
+				{
+					stkStack.resize(stkStack.size()*2);
+					treshold=stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(b3Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+
+
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTTpersistentStack(	const b3DbvtNode* root0,
+								  const b3DbvtNode* root1,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=B3_DOUBLE_STACKSIZE-4;
+			
+			m_stkStack.resize(B3_DOUBLE_STACKSIZE);
+			m_stkStack[0]=sStkNN(root0,root1);
+			do	{		
+				sStkNN	p=m_stkStack[--depth];
+				if(depth>treshold)
+				{
+					m_stkStack.resize(m_stkStack.size()*2);
+					treshold=m_stkStack.size()-4;
+				}
+				if(p.a==p.b)
+				{
+					if(p.a->isinternal())
+					{
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
+						m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
+					}
+				}
+				else if(b3Intersect(p.a->volume,p.b->volume))
+				{
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+
+#if 0
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
+								  const b3DbvtNode* root1,
+								  const b3Transform& xform,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root0&&root1)
+		{
+			int								depth=1;
+			int								treshold=B3_DOUBLE_STACKSIZE-4;
+			b3AlignedObjectArray<sStkNN>	stkStack;
+			stkStack.resize(B3_DOUBLE_STACKSIZE);
+			stkStack[0]=sStkNN(root0,root1);
+			do	{
+				sStkNN	p=stkStack[--depth];
+				if(b3Intersect(p.a->volume,p.b->volume,xform))
+				{
+					if(depth>treshold)
+					{
+						stkStack.resize(stkStack.size()*2);
+						treshold=stkStack.size()-4;
+					}
+					if(p.a->isinternal())
+					{
+						if(p.b->isinternal())
+						{					
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
+						}
+						else
+						{
+							stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
+							stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
+						}
+					}
+					else
+					{
+						if(p.b->isinternal())
+						{
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
+							stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
+						}
+						else
+						{
+							policy.Process(p.a,p.b);
+						}
+					}
+				}
+			} while(depth);
+		}
+}
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTT(	const b3DbvtNode* root0,
+								  const b3Transform& xform0,
+								  const b3DbvtNode* root1,
+								  const b3Transform& xform1,
+								  B3_DBVT_IPOLICY)
+{
+	const b3Transform	xform=xform0.inverse()*xform1;
+	collideTT(root0,root1,xform,policy);
+}
+#endif 
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTV(	const b3DbvtNode* root,
+								  const b3DbvtVolume& vol,
+								  B3_DBVT_IPOLICY) const
+{
+	B3_DBVT_CHECKTYPE
+		if(root)
+		{
+			B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)		volume(vol);
+			b3AlignedObjectArray<const b3DbvtNode*>	stack;
+			stack.resize(0);
+			stack.reserve(B3_SIMPLE_STACKSIZE);
+			stack.push_back(root);
+			do	{
+				const b3DbvtNode*	n=stack[stack.size()-1];
+				stack.pop_back();
+				if(b3Intersect(n->volume,volume))
+				{
+					if(n->isinternal())
+					{
+						stack.push_back(n->childs[0]);
+						stack.push_back(n->childs[1]);
+					}
+					else
+					{
+						policy.Process(n);
+					}
+				}
+			} while(stack.size()>0);
+		}
+}
+
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::rayTestInternal(	const b3DbvtNode* root,
+								const b3Vector3& rayFrom,
+								const b3Vector3& rayTo,
+								const b3Vector3& rayDirectionInverse,
+								unsigned int signs[3],
+								b3Scalar lambda_max,
+								const b3Vector3& aabbMin,
+								const b3Vector3& aabbMax,
+								B3_DBVT_IPOLICY) const
+{
+        (void) rayTo;
+	B3_DBVT_CHECKTYPE
+	if(root)
+	{
+		int								depth=1;
+		int								treshold=B3_DOUBLE_STACKSIZE-2;
+		b3AlignedObjectArray<const b3DbvtNode*>&	stack = m_rayTestStack;
+		stack.resize(B3_DOUBLE_STACKSIZE);
+		stack[0]=root;
+		b3Vector3 bounds[2];
+		do	
+		{
+			const b3DbvtNode*	node=stack[--depth];
+			bounds[0] = node->volume.Mins()-aabbMax;
+			bounds[1] = node->volume.Maxs()-aabbMin;
+			b3Scalar tmin=1.f,lambda_min=0.f;
+			unsigned int result1=false;
+			result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+			if(result1)
+			{
+				if(node->isinternal())
+				{
+					if(depth>treshold)
+					{
+						stack.resize(stack.size()*2);
+						treshold=stack.size()-2;
+					}
+					stack[depth++]=node->childs[0];
+					stack[depth++]=node->childs[1];
+				}
+				else
+				{
+					policy.Process(node);
+				}
+			}
+		} while(depth);
+	}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::rayTest(	const b3DbvtNode* root,
+								const b3Vector3& rayFrom,
+								const b3Vector3& rayTo,
+								B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root)
+		{
+			b3Vector3 rayDir = (rayTo-rayFrom);
+			rayDir.normalize ();
+
+			///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
+			b3Vector3 rayDirectionInverse;
+			rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0];
+			rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1];
+			rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2];
+			unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+
+			b3Scalar lambda_max = rayDir.dot(rayTo-rayFrom);
+#ifdef COMPARE_BTRAY_AABB2
+			b3Vector3 resultNormal;
+#endif//COMPARE_BTRAY_AABB2
+			
+			b3AlignedObjectArray<const b3DbvtNode*>	stack;
+
+			int								depth=1;
+			int								treshold=B3_DOUBLE_STACKSIZE-2;
+
+			stack.resize(B3_DOUBLE_STACKSIZE);
+			stack[0]=root;
+			b3Vector3 bounds[2];
+			do	{
+				const b3DbvtNode*	node=stack[--depth];
+
+				bounds[0] = node->volume.Mins();
+				bounds[1] = node->volume.Maxs();
+				
+				b3Scalar tmin=1.f,lambda_min=0.f;
+				unsigned int result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
+
+#ifdef COMPARE_BTRAY_AABB2
+				b3Scalar param=1.f;
+				bool result2 = b3RayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
+				b3Assert(result1 == result2);
+#endif //TEST_BTRAY_AABB2
+
+				if(result1)
+				{
+					if(node->isinternal())
+					{
+						if(depth>treshold)
+						{
+							stack.resize(stack.size()*2);
+							treshold=stack.size()-2;
+						}
+						stack[depth++]=node->childs[0];
+						stack[depth++]=node->childs[1];
+					}
+					else
+					{
+						policy.Process(node);
+					}
+				}
+			} while(depth);
+
+		}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
+									const b3Vector3* normals,
+									const b3Scalar* offsets,
+									int count,
+									B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root)
+		{
+			const int						inside=(1<<count)-1;
+			b3AlignedObjectArray<sStkNP>	stack;
+			int								signs[sizeof(unsigned)*8];
+			b3Assert(count<int (sizeof(signs)/sizeof(signs[0])));
+			for(int i=0;i<count;++i)
+			{
+				signs[i]=	((normals[i].x>=0)?1:0)+
+					((normals[i].y>=0)?2:0)+
+					((normals[i].z>=0)?4:0);
+			}
+			stack.reserve(B3_SIMPLE_STACKSIZE);
+			stack.push_back(sStkNP(root,0));
+			do	{
+				sStkNP	se=stack[stack.size()-1];
+				bool	out=false;
+				stack.pop_back();
+				for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+				{
+					if(0==(se.mask&j))
+					{
+						const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+						switch(side)
+						{
+						case	-1:	out=true;break;
+						case	+1:	se.mask|=j;break;
+						}
+					}
+				}
+				if(!out)
+				{
+					if((se.mask!=inside)&&(se.node->isinternal()))
+					{
+						stack.push_back(sStkNP(se.node->childs[0],se.mask));
+						stack.push_back(sStkNP(se.node->childs[1],se.mask));
+					}
+					else
+					{
+						if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
+					}
+				}
+			} while(stack.size());
+		}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideOCL(	const b3DbvtNode* root,
+								   const b3Vector3* normals,
+								   const b3Scalar* offsets,
+								   const b3Vector3& sortaxis,
+								   int count,
+								   B3_DBVT_IPOLICY,
+								   bool fsort)
+{
+	B3_DBVT_CHECKTYPE
+		if(root)
+		{
+			const unsigned					srtsgns=(sortaxis[0]>=0?1:0)+
+				(sortaxis[1]>=0?2:0)+
+				(sortaxis[2]>=0?4:0);
+			const int						inside=(1<<count)-1;
+			b3AlignedObjectArray<sStkNPS>	stock;
+			b3AlignedObjectArray<int>		ifree;
+			b3AlignedObjectArray<int>		stack;
+			int								signs[sizeof(unsigned)*8];
+			b3Assert(count<int (sizeof(signs)/sizeof(signs[0])));
+			for(int i=0;i<count;++i)
+			{
+				signs[i]=	((normals[i].x>=0)?1:0)+
+					((normals[i].y>=0)?2:0)+
+					((normals[i].z>=0)?4:0);
+			}
+			stock.reserve(B3_SIMPLE_STACKSIZE);
+			stack.reserve(B3_SIMPLE_STACKSIZE);
+			ifree.reserve(B3_SIMPLE_STACKSIZE);
+			stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
+			do	{
+				const int	id=stack[stack.size()-1];
+				sStkNPS		se=stock[id];
+				stack.pop_back();ifree.push_back(id);
+				if(se.mask!=inside)
+				{
+					bool	out=false;
+					for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+					{
+						if(0==(se.mask&j))
+						{
+							const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+							switch(side)
+							{
+							case	-1:	out=true;break;
+							case	+1:	se.mask|=j;break;
+							}
+						}
+					}
+					if(out) continue;
+				}
+				if(policy.Descent(se.node))
+				{
+					if(se.node->isinternal())
+					{
+						const b3DbvtNode* pns[]={	se.node->childs[0],se.node->childs[1]};
+						sStkNPS		nes[]={	sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
+							sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
+						const int	q=nes[0].value<nes[1].value?1:0;				
+						int			j=stack.size();
+						if(fsort&&(j>0))
+						{
+							/* Insert 0	*/ 
+							j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
+							stack.push_back(0);
+#if B3_DBVT_USE_MEMMOVE
+							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+							stack[j]=allocate(ifree,stock,nes[q]);
+							/* Insert 1	*/ 
+							j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
+							stack.push_back(0);
+#if B3_DBVT_USE_MEMMOVE
+							memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
+#else
+							for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
+#endif
+							stack[j]=allocate(ifree,stock,nes[1-q]);
+						}
+						else
+						{
+							stack.push_back(allocate(ifree,stock,nes[q]));
+							stack.push_back(allocate(ifree,stock,nes[1-q]));
+						}
+					}
+					else
+					{
+						policy.Process(se.node,se.value);
+					}
+				}
+			} while(stack.size());
+		}
+}
+
+//
+B3_DBVT_PREFIX
+inline void		b3DynamicBvh::collideTU(	const b3DbvtNode* root,
+								  B3_DBVT_IPOLICY)
+{
+	B3_DBVT_CHECKTYPE
+		if(root)
+		{
+			b3AlignedObjectArray<const b3DbvtNode*>	stack;
+			stack.reserve(B3_SIMPLE_STACKSIZE);
+			stack.push_back(root);
+			do	{
+				const b3DbvtNode*	n=stack[stack.size()-1];
+				stack.pop_back();
+				if(policy.Descent(n))
+				{
+					if(n->isinternal())
+					{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
+					else
+					{ policy.Process(n); }
+				}
+			} while(stack.size()>0);
+		}
+}
+
+//
+// PP Cleanup
+//
+
+#undef B3_DBVT_USE_MEMMOVE
+#undef B3_DBVT_USE_TEMPLATE
+#undef B3_DBVT_VIRTUAL_DTOR
+#undef B3_DBVT_VIRTUAL
+#undef B3_DBVT_PREFIX
+#undef B3_DBVT_IPOLICY
+#undef B3_DBVT_CHECKTYPE
+#undef B3_DBVT_IMPL_GENERIC
+#undef B3_DBVT_IMPL_SSE
+#undef B3_DBVT_USE_INTRINSIC_SSE
+#undef B3_DBVT_SELECT_IMPL
+#undef B3_DBVT_MERGE_IMPL
+#undef B3_DBVT_INT0_IMPL
+
+#endif

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp

@@ -0,0 +1,804 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+
+#include "b3DynamicBvhBroadphase.h"
+#include "b3OverlappingPair.h"
+
+//
+// Profiling
+//
+
+#if B3_DBVT_BP_PROFILE||B3_DBVT_BP_ENABLE_BENCHMARK
+#include <stdio.h>
+#endif
+
+#if B3_DBVT_BP_PROFILE
+struct	b3ProfileScope
+{
+	__forceinline b3ProfileScope(b3Clock& clock,unsigned long& value) :
+	m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
+	{
+	}
+	__forceinline ~b3ProfileScope()
+	{
+		(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
+	}
+	b3Clock*		m_clock;
+	unsigned long*	m_value;
+	unsigned long	m_base;
+};
+#define	b3SPC(_value_)	b3ProfileScope	spc_scope(m_clock,_value_)
+#else
+#define	b3SPC(_value_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void	b3ListAppend(T* item,T*& list)
+{
+	item->links[0]=0;
+	item->links[1]=list;
+	if(list) list->links[0]=item;
+	list=item;
+}
+
+//
+template <typename T>
+static inline void	b3ListRemove(T* item,T*& list)
+{
+	if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
+	if(item->links[1]) item->links[1]->links[0]=item->links[0];
+}
+
+//
+template <typename T>
+static inline int	b3ListCount(T* root)
+{
+	int	n=0;
+	while(root) { ++n;root=root->links[1]; }
+	return(n);
+}
+
+//
+template <typename T>
+static inline void	b3Clear(T& value)
+{
+	static const struct ZeroDummy : T {} zerodummy;
+	value=zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider	*/ 
+struct	b3DbvtTreeCollider : b3DynamicBvh::ICollide
+{
+	b3DynamicBvhBroadphase*	pbp;
+	b3DbvtProxy*		proxy;
+	b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
+	void	Process(const b3DbvtNode* na,const b3DbvtNode* nb)
+	{
+		if(na!=nb)
+		{
+			b3DbvtProxy*	pa=(b3DbvtProxy*)na->data;
+			b3DbvtProxy*	pb=(b3DbvtProxy*)nb->data;
+#if B3_DBVT_BP_SORTPAIRS
+			if(pa->m_uniqueId>pb->m_uniqueId) 
+				b3Swap(pa,pb);
+#endif
+			pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid());
+			++pbp->m_newpairs;
+		}
+	}
+	void	Process(const b3DbvtNode* n)
+	{
+		Process(n,proxy->leaf);
+	}
+};
+
+//
+// b3DynamicBvhBroadphase
+//
+
+//
+b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache)
+{
+	m_deferedcollide	=	false;
+	m_needcleanup		=	true;
+	m_releasepaircache	=	(paircache!=0)?false:true;
+	m_prediction		=	0;
+	m_stageCurrent		=	0;
+	m_fixedleft			=	0;
+	m_fupdates			=	1;
+	m_dupdates			=	0;
+	m_cupdates			=	10;
+	m_newpairs			=	1;
+	m_updates_call		=	0;
+	m_updates_done		=	0;
+	m_updates_ratio		=	0;
+	m_paircache			=	paircache? paircache	: new(b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache),16)) b3HashedOverlappingPairCache();
+	
+	m_pid				=	0;
+	m_cid				=	0;
+	for(int i=0;i<=STAGECOUNT;++i)
+	{
+		m_stageRoots[i]=0;
+	}
+#if B3_DBVT_BP_PROFILE
+	b3Clear(m_profiling);
+#endif
+	m_proxies.resize(proxyCapacity);
+}
+
+//
+b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
+{
+	if(m_releasepaircache) 
+	{
+		m_paircache->~b3OverlappingPairCache();
+		b3AlignedFree(m_paircache);
+	}
+}
+
+//
+b3BroadphaseProxy*				b3DynamicBvhBroadphase::createProxy(	const b3Vector3& aabbMin,
+															  const b3Vector3& aabbMax,
+															  int objectId,
+															  void* userPtr,
+															   int collisionFilterGroup,
+																int collisionFilterMask)
+{
+	b3DbvtProxy* mem = &m_proxies[objectId];
+	b3DbvtProxy*		proxy=new(mem) b3DbvtProxy(	aabbMin,aabbMax,userPtr,
+		collisionFilterGroup,
+		collisionFilterMask);
+
+	b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
+
+	//bproxy->aabb			=	b3DbvtVolume::FromMM(aabbMin,aabbMax);
+	proxy->stage		=	m_stageCurrent;
+	proxy->m_uniqueId	=	objectId;
+	proxy->leaf			=	m_sets[0].insert(aabb,proxy);
+	b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
+	if(!m_deferedcollide)
+	{
+		b3DbvtTreeCollider	collider(this);
+		collider.proxy=proxy;
+		m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
+		m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
+	}
+	return(proxy);
+}
+
+//
+void							b3DynamicBvhBroadphase::destroyProxy(	b3BroadphaseProxy* absproxy,
+															   b3Dispatcher* dispatcher)
+{
+	b3DbvtProxy*	proxy=(b3DbvtProxy*)absproxy;
+	if(proxy->stage==STAGECOUNT)
+		m_sets[1].remove(proxy->leaf);
+	else
+		m_sets[0].remove(proxy->leaf);
+	b3ListRemove(proxy,m_stageRoots[proxy->stage]);
+	m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(),dispatcher);
+	
+	m_needcleanup=true;
+}
+
+void	b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
+{
+	const b3DbvtProxy*						proxy=&m_proxies[objectId];
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+/*
+void	b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
+{
+	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+*/
+
+
+struct	BroadphaseRayTester : b3DynamicBvh::ICollide
+{
+	b3BroadphaseRayCallback& m_rayCallback;
+	BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
+		:m_rayCallback(orgCallback)
+	{
+	}
+	void					Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy*	proxy=(b3DbvtProxy*)leaf->data;
+		m_rayCallback.process(proxy);
+	}
+};	
+
+void	b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax)
+{
+	BroadphaseRayTester callback(rayCallback);
+
+	m_sets[0].rayTestInternal(	m_sets[0].m_root,
+		rayFrom,
+		rayTo,
+		rayCallback.m_rayDirectionInverse,
+		rayCallback.m_signs,
+		rayCallback.m_lambda_max,
+		aabbMin,
+		aabbMax,
+		callback);
+
+	m_sets[1].rayTestInternal(	m_sets[1].m_root,
+		rayFrom,
+		rayTo,
+		rayCallback.m_rayDirectionInverse,
+		rayCallback.m_signs,
+		rayCallback.m_lambda_max,
+		aabbMin,
+		aabbMax,
+		callback);
+
+}
+
+
+struct	BroadphaseAabbTester : b3DynamicBvh::ICollide
+{
+	b3BroadphaseAabbCallback& m_aabbCallback;
+	BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
+		:m_aabbCallback(orgCallback)
+	{
+	}
+	void					Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy*	proxy=(b3DbvtProxy*)leaf->data;
+		m_aabbCallback.process(proxy);
+	}
+};	
+
+void	b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3BroadphaseAabbCallback& aabbCallback)
+{
+	BroadphaseAabbTester callback(aabbCallback);
+
+	const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)	bounds=b3DbvtVolume::FromMM(aabbMin,aabbMax);
+		//process all children, that overlap with  the given AABB bounds
+	m_sets[0].collideTV(m_sets[0].m_root,bounds,callback);
+	m_sets[1].collideTV(m_sets[1].m_root,bounds,callback);
+
+}
+
+
+
+//
+void							b3DynamicBvhBroadphase::setAabb(int objectId,
+														  const b3Vector3& aabbMin,
+														  const b3Vector3& aabbMax,
+														  b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy*						proxy=&m_proxies[objectId];
+//	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)	aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
+#if B3_DBVT_BP_PREVENTFALSEUPDATE
+	if(b3NotEqual(aabb,proxy->leaf->volume))
+#endif
+	{
+		bool	docollide=false;
+		if(proxy->stage==STAGECOUNT)
+		{/* fixed -> dynamic set	*/ 
+			m_sets[1].remove(proxy->leaf);
+			proxy->leaf=m_sets[0].insert(aabb,proxy);
+			docollide=true;
+		}
+		else
+		{/* dynamic set				*/ 
+			++m_updates_call;
+			if(b3Intersect(proxy->leaf->volume,aabb))
+			{/* Moving				*/ 
+
+				const b3Vector3	delta=aabbMin-proxy->m_aabbMin;
+				b3Vector3		velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction);
+				if(delta[0]<0) velocity[0]=-velocity[0];
+				if(delta[1]<0) velocity[1]=-velocity[1];
+				if(delta[2]<0) velocity[2]=-velocity[2];
+				if	(
+#ifdef B3_DBVT_BP_MARGIN				
+					m_sets[0].update(proxy->leaf,aabb,velocity,B3_DBVT_BP_MARGIN)
+#else
+					m_sets[0].update(proxy->leaf,aabb,velocity)
+#endif
+					)
+				{
+					++m_updates_done;
+					docollide=true;
+				}
+			}
+			else
+			{/* Teleporting			*/ 
+				m_sets[0].update(proxy->leaf,aabb);
+				++m_updates_done;
+				docollide=true;
+			}	
+		}
+		b3ListRemove(proxy,m_stageRoots[proxy->stage]);
+		proxy->m_aabbMin = aabbMin;
+		proxy->m_aabbMax = aabbMax;
+		proxy->stage	=	m_stageCurrent;
+		b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
+		if(docollide)
+		{
+			m_needcleanup=true;
+			if(!m_deferedcollide)
+			{
+				b3DbvtTreeCollider	collider(this);
+				m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
+				m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+			}
+		}	
+	}
+}
+
+
+//
+void							b3DynamicBvhBroadphase::setAabbForceUpdate(		b3BroadphaseProxy* absproxy,
+														  const b3Vector3& aabbMin,
+														  const b3Vector3& aabbMax,
+														  b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)	aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
+	bool	docollide=false;
+	if(proxy->stage==STAGECOUNT)
+	{/* fixed -> dynamic set	*/ 
+		m_sets[1].remove(proxy->leaf);
+		proxy->leaf=m_sets[0].insert(aabb,proxy);
+		docollide=true;
+	}
+	else
+	{/* dynamic set				*/ 
+		++m_updates_call;
+		/* Teleporting			*/ 
+		m_sets[0].update(proxy->leaf,aabb);
+		++m_updates_done;
+		docollide=true;
+	}
+	b3ListRemove(proxy,m_stageRoots[proxy->stage]);
+	proxy->m_aabbMin = aabbMin;
+	proxy->m_aabbMax = aabbMax;
+	proxy->stage	=	m_stageCurrent;
+	b3ListAppend(proxy,m_stageRoots[m_stageCurrent]);
+	if(docollide)
+	{
+		m_needcleanup=true;
+		if(!m_deferedcollide)
+		{
+			b3DbvtTreeCollider	collider(this);
+			m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
+		}
+	}	
+}
+
+//
+void							b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	collide(dispatcher);
+#if B3_DBVT_BP_PROFILE
+	if(0==(m_pid%B3_DBVT_BP_PROFILING_RATE))
+	{	
+		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs());
+		unsigned int	total=m_profiling.m_total;
+		if(total<=0) total=1;
+		printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/B3_DBVT_BP_PROFILING_RATE);
+		printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/B3_DBVT_BP_PROFILING_RATE);
+		printf("cleanup:   %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/B3_DBVT_BP_PROFILING_RATE);
+		printf("total:     %uus\r\n",total/B3_DBVT_BP_PROFILING_RATE);
+		const unsigned long	sum=m_profiling.m_ddcollide+
+			m_profiling.m_fdcollide+
+			m_profiling.m_cleanup;
+		printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/B3_DBVT_BP_PROFILING_RATE);
+		printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*B3_DBVT_BP_PROFILING_RATE));
+		b3Clear(m_profiling);
+		m_clock.reset();
+	}
+#endif
+
+	performDeferredRemoval(dispatcher);
+
+}
+
+void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
+{
+
+	if (m_paircache->hasDeferredRemoval())
+	{
+
+		b3BroadphasePairArray&	overlappingPairArray = m_paircache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+
+		int invalidPair = 0;
+
+		
+		int i;
+
+		b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1);
+		
+		
+		
+		for (i=0;i<overlappingPairArray.size();i++)
+		{
+		
+			b3BroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				//important to perform AABB check that is consistent with the broadphase
+				b3DbvtProxy*		pa=&m_proxies[pair.x];
+				b3DbvtProxy*		pb=&m_proxies[pair.y];
+				bool hasOverlap = b3Intersect(pa->leaf->volume,pb->leaf->volume);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				} else
+				{
+					needsRemoval = true;
+				}
+			} else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+			}
+			
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair,dispatcher);
+
+				pair.x = -1;
+				pair.y = -1;
+				invalidPair++;
+			} 
+			
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
+}
+
+//
+void							b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
+{
+	/*printf("---------------------------------------------------------\n");
+	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+	{
+		int i;
+		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+		{
+			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+		}
+		printf("\n");
+	}
+*/
+
+
+
+	b3SPC(m_profiling.m_total);
+	/* optimize				*/ 
+	m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
+	if(m_fixedleft)
+	{
+		const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
+		m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
+		m_fixedleft=b3Max<int>(0,m_fixedleft-count);
+	}
+	/* dynamic -> fixed set	*/ 
+	m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
+	b3DbvtProxy*	current=m_stageRoots[m_stageCurrent];
+	if(current)
+	{
+		b3DbvtTreeCollider	collider(this);
+		do	{
+			b3DbvtProxy*	next=current->links[1];
+			b3ListRemove(current,m_stageRoots[current->stage]);
+			b3ListAppend(current,m_stageRoots[STAGECOUNT]);
+#if B3_DBVT_BP_ACCURATESLEEPING
+			m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher);
+			collider.proxy=current;
+			b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider);
+			b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider);
+#endif
+			m_sets[0].remove(current->leaf);
+			B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)	curAabb=b3DbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
+			current->leaf	=	m_sets[1].insert(curAabb,current);
+			current->stage	=	STAGECOUNT;	
+			current			=	next;
+		} while(current);
+		m_fixedleft=m_sets[1].m_leaves;
+		m_needcleanup=true;
+	}
+	/* collide dynamics		*/ 
+	{
+		b3DbvtTreeCollider	collider(this);
+		if(m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_fdcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider);
+		}
+		if(m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_ddcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider);
+		}
+	}
+	/* clean up				*/ 
+	if(m_needcleanup)
+	{
+		b3SPC(m_profiling.m_cleanup);
+		b3BroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
+		if(pairs.size()>0)
+		{
+
+			int			ni=b3Min(pairs.size(),b3Max<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
+			for(int i=0;i<ni;++i)
+			{
+				b3BroadphasePair&	p=pairs[(m_cid+i)%pairs.size()];
+				b3DbvtProxy*		pa=&m_proxies[p.x];
+				b3DbvtProxy*		pb=&m_proxies[p.y];
+				if(!b3Intersect(pa->leaf->volume,pb->leaf->volume))
+				{
+#if B3_DBVT_BP_SORTPAIRS
+					if(pa->m_uniqueId>pb->m_uniqueId) 
+						b3Swap(pa,pb);
+#endif
+					m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher);
+					--ni;--i;
+				}
+			}
+			if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
+		}
+	}
+	++m_pid;
+	m_newpairs=1;
+	m_needcleanup=false;
+	if(m_updates_call>0)
+	{ m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; }
+	else
+	{ m_updates_ratio=0; }
+	m_updates_done/=2;
+	m_updates_call/=2;
+}
+
+//
+void							b3DynamicBvhBroadphase::optimize()
+{
+	m_sets[0].optimizeTopDown();
+	m_sets[1].optimizeTopDown();
+}
+
+//
+b3OverlappingPairCache*			b3DynamicBvhBroadphase::getOverlappingPairCache()
+{
+	return(m_paircache);
+}
+
+//
+const b3OverlappingPairCache*	b3DynamicBvhBroadphase::getOverlappingPairCache() const
+{
+	return(m_paircache);
+}
+
+//
+void							b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const
+{
+
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)	bounds;
+
+	if(!m_sets[0].empty())
+		if(!m_sets[1].empty())	b3Merge(	m_sets[0].m_root->volume,
+			m_sets[1].m_root->volume,bounds);
+		else
+			bounds=m_sets[0].m_root->volume;
+	else if(!m_sets[1].empty())	bounds=m_sets[1].m_root->volume;
+	else
+		bounds=b3DbvtVolume::FromCR(b3MakeVector3(0,0,0),0);
+	aabbMin=bounds.Mins();
+	aabbMax=bounds.Maxs();
+}
+
+void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
+{
+	
+	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
+	if (!totalObjects)
+	{
+		//reset internal dynamic tree data structures
+		m_sets[0].clear();
+		m_sets[1].clear();
+		
+		m_deferedcollide	=	false;
+		m_needcleanup		=	true;
+		m_stageCurrent		=	0;
+		m_fixedleft			=	0;
+		m_fupdates			=	1;
+		m_dupdates			=	0;
+		m_cupdates			=	10;
+		m_newpairs			=	1;
+		m_updates_call		=	0;
+		m_updates_done		=	0;
+		m_updates_ratio		=	0;
+		
+		m_pid				=	0;
+		m_cid				=	0;
+		for(int i=0;i<=STAGECOUNT;++i)
+		{
+			m_stageRoots[i]=0;
+		}
+	}
+}
+
+//
+void							b3DynamicBvhBroadphase::printStats()
+{}
+
+//
+#if B3_DBVT_BP_ENABLE_BENCHMARK
+
+struct	b3BroadphaseBenchmark
+{
+	struct	Experiment
+	{
+		const char*			name;
+		int					object_count;
+		int					update_count;
+		int					spawn_count;
+		int					iterations;
+		b3Scalar			speed;
+		b3Scalar			amplitude;
+	};
+	struct	Object
+	{
+		b3Vector3			center;
+		b3Vector3			extents;
+		b3BroadphaseProxy*	proxy;
+		b3Scalar			time;
+		void				update(b3Scalar speed,b3Scalar amplitude,b3BroadphaseInterface* pbi)
+		{
+			time		+=	speed;
+			center[0]	=	b3Cos(time*(b3Scalar)2.17)*amplitude+
+				b3Sin(time)*amplitude/2;
+			center[1]	=	b3Cos(time*(b3Scalar)1.38)*amplitude+
+				b3Sin(time)*amplitude;
+			center[2]	=	b3Sin(time*(b3Scalar)0.777)*amplitude;
+			pbi->setAabb(proxy,center-extents,center+extents,0);
+		}
+	};
+	static int		UnsignedRand(int range=RAND_MAX-1)	{ return(rand()%(range+1)); }
+	static b3Scalar	UnitRand()							{ return(UnsignedRand(16384)/(b3Scalar)16384); }
+	static void		OutputTime(const char* name,b3Clock& c,unsigned count=0)
+	{
+		const unsigned long	us=c.getTimeMicroseconds();
+		const unsigned long	ms=(us+500)/1000;
+		const b3Scalar		sec=us/(b3Scalar)(1000*1000);
+		if(count>0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec);
+		else
+			printf("%s : %u us (%u ms)\r\n",name,us,ms);
+	}
+};
+
+void							b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
+{
+	static const b3BroadphaseBenchmark::Experiment		experiments[]=
+	{
+		{"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
+		/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
+		{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
+	};
+	static const int										nexperiments=sizeof(experiments)/sizeof(experiments[0]);
+	b3AlignedObjectArray<b3BroadphaseBenchmark::Object*>	objects;
+	b3Clock													wallclock;
+	/* Begin			*/ 
+	for(int iexp=0;iexp<nexperiments;++iexp)
+	{
+		const b3BroadphaseBenchmark::Experiment&	experiment=experiments[iexp];
+		const int									object_count=experiment.object_count;
+		const int									update_count=(object_count*experiment.update_count)/100;
+		const int									spawn_count=(object_count*experiment.spawn_count)/100;
+		const b3Scalar								speed=experiment.speed;	
+		const b3Scalar								amplitude=experiment.amplitude;
+		printf("Experiment #%u '%s':\r\n",iexp,experiment.name);
+		printf("\tObjects: %u\r\n",object_count);
+		printf("\tUpdate: %u\r\n",update_count);
+		printf("\tSpawn: %u\r\n",spawn_count);
+		printf("\tSpeed: %f\r\n",speed);
+		printf("\tAmplitude: %f\r\n",amplitude);
+		srand(180673);
+		/* Create objects	*/ 
+		wallclock.reset();
+		objects.reserve(object_count);
+		for(int i=0;i<object_count;++i)
+		{
+			b3BroadphaseBenchmark::Object*	po=new b3BroadphaseBenchmark::Object();
+			po->center[0]=b3BroadphaseBenchmark::UnitRand()*50;
+			po->center[1]=b3BroadphaseBenchmark::UnitRand()*50;
+			po->center[2]=b3BroadphaseBenchmark::UnitRand()*50;
+			po->extents[0]=b3BroadphaseBenchmark::UnitRand()*2+2;
+			po->extents[1]=b3BroadphaseBenchmark::UnitRand()*2+2;
+			po->extents[2]=b3BroadphaseBenchmark::UnitRand()*2+2;
+			po->time=b3BroadphaseBenchmark::UnitRand()*2000;
+			po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
+			objects.push_back(po);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
+		/* First update		*/ 
+		wallclock.reset();
+		for(int i=0;i<objects.size();++i)
+		{
+			objects[i]->update(speed,amplitude,pbi);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
+		/* Updates			*/ 
+		wallclock.reset();
+		for(int i=0;i<experiment.iterations;++i)
+		{
+			for(int j=0;j<update_count;++j)
+			{				
+				objects[j]->update(speed,amplitude,pbi);
+			}
+			pbi->calculateOverlappingPairs(0);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
+		/* Clean up			*/ 
+		wallclock.reset();
+		for(int i=0;i<objects.size();++i)
+		{
+			pbi->destroyProxy(objects[i]->proxy,0);
+			delete objects[i];
+		}
+		objects.resize(0);
+		b3BroadphaseBenchmark::OutputTime("\tRelease",wallclock);
+	}
+
+}
+#else
+/*void							b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
+{}
+*/
+#endif
+
+#if B3_DBVT_BP_PROFILE
+#undef	b3SPC
+#endif
+

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h

@@ -0,0 +1,206 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+#ifndef B3_DBVT_BROADPHASE_H
+#define B3_DBVT_BROADPHASE_H
+
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include "b3BroadphaseCallback.h"
+
+//
+// Compile time config
+//
+
+#define	B3_DBVT_BP_PROFILE					0
+//#define B3_DBVT_BP_SORTPAIRS				1
+#define B3_DBVT_BP_PREVENTFALSEUPDATE		0
+#define B3_DBVT_BP_ACCURATESLEEPING		0
+#define B3_DBVT_BP_ENABLE_BENCHMARK		0
+#define B3_DBVT_BP_MARGIN					(b3Scalar)0.05
+
+#if B3_DBVT_BP_PROFILE
+#define	B3_DBVT_BP_PROFILING_RATE	256
+
+#endif
+
+
+
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
+{
+
+B3_DECLARE_ALIGNED_ALLOCATOR();
+	
+	///optional filtering to cull potential collisions
+	enum CollisionFilterGroups
+	{
+	        DefaultFilter = 1,
+	        StaticFilter = 2,
+	        KinematicFilter = 4,
+	        DebrisFilter = 8,
+			SensorTrigger = 16,
+			CharacterFilter = 32,
+	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+	};
+
+	//Usually the client b3CollisionObject or Rigidbody class
+	void*	m_clientObject;
+	int m_collisionFilterGroup;
+	int m_collisionFilterMask;
+	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	b3Vector3	m_aabbMin;
+	b3Vector3	m_aabbMax;
+
+	B3_FORCE_INLINE int getUid() const
+	{
+		return m_uniqueId;
+	}
+
+	//used for memory pools
+	b3BroadphaseProxy() :m_clientObject(0)
+	{
+	}
+
+	b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask)
+		:m_clientObject(userPtr),
+		m_collisionFilterGroup(collisionFilterGroup),
+		m_collisionFilterMask(collisionFilterMask),
+		m_aabbMin(aabbMin),
+		m_aabbMax(aabbMax)
+	{
+	}
+};
+
+
+
+
+
+//
+// b3DbvtProxy
+//
+struct b3DbvtProxy : b3BroadphaseProxy
+{
+	/* Fields		*/ 
+	//b3DbvtAabbMm	aabb;
+	b3DbvtNode*		leaf;
+	b3DbvtProxy*	links[2];
+	int				stage;
+	/* ctor			*/ 
+
+	explicit b3DbvtProxy() {}
+	b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask) :
+	b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
+	{
+		links[0]=links[1]=0;
+	}
+};
+
+typedef b3AlignedObjectArray<b3DbvtProxy*>	b3DbvtProxyArray;
+
+///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
+///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
+///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
+struct	b3DynamicBvhBroadphase 
+{
+	/* Config		*/ 
+	enum	{
+		DYNAMIC_SET			=	0,	/* Dynamic set index	*/ 
+		FIXED_SET			=	1,	/* Fixed set index		*/ 
+		STAGECOUNT			=	2	/* Number of stages		*/ 
+	};
+	/* Fields		*/ 
+	b3DynamicBvh					m_sets[2];					// Dbvt sets
+	b3DbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
+
+	b3AlignedObjectArray<b3DbvtProxy>	m_proxies;
+	b3OverlappingPairCache*	m_paircache;				// Pair cache
+	b3Scalar				m_prediction;				// Velocity prediction
+	int						m_stageCurrent;				// Current stage
+	int						m_fupdates;					// % of fixed updates per frame
+	int						m_dupdates;					// % of dynamic updates per frame
+	int						m_cupdates;					// % of cleanup updates per frame
+	int						m_newpairs;					// Number of pairs created
+	int						m_fixedleft;				// Fixed optimization left
+	unsigned				m_updates_call;				// Number of updates call
+	unsigned				m_updates_done;				// Number of updates done
+	b3Scalar				m_updates_ratio;			// m_updates_done/m_updates_call
+	int						m_pid;						// Parse id
+	int						m_cid;						// Cleanup index
+	bool					m_releasepaircache;			// Release pair cache on delete
+	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
+	bool					m_needcleanup;				// Need to run cleanup?
+#if B3_DBVT_BP_PROFILE
+	b3Clock					m_clock;
+	struct	{
+		unsigned long		m_total;
+		unsigned long		m_ddcollide;
+		unsigned long		m_fdcollide;
+		unsigned long		m_cleanup;
+		unsigned long		m_jobcount;
+	}				m_profiling;
+#endif
+	/* Methods		*/ 
+	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
+	virtual ~b3DynamicBvhBroadphase();
+	void							collide(b3Dispatcher* dispatcher);
+	void							optimize();
+	
+	/* b3BroadphaseInterface Implementation	*/
+	b3BroadphaseProxy*				createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void					destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
+	virtual void					setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
+	virtual void					rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
+	virtual void					aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
+
+	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	virtual void					getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	virtual	void					calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
+	virtual	b3OverlappingPairCache*	getOverlappingPairCache();
+	virtual	const b3OverlappingPairCache*	getOverlappingPairCache() const;
+	virtual	void					getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const;
+	virtual	void					printStats();
+
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(b3Dispatcher* dispatcher);
+
+	void	performDeferredRemoval(b3Dispatcher* dispatcher);
+	
+	void	setVelocityPrediction(b3Scalar prediction)
+	{
+		m_prediction = prediction;
+	}
+	b3Scalar getVelocityPrediction() const
+	{
+		return m_prediction;
+	}
+
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. 
+	///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
+	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
+	///http://code.google.com/p/bullet/issues/detail?id=223
+	void							setAabbForceUpdate(		b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/);
+
+	//static void						benchmark(b3BroadphaseInterface*);
+
+
+};
+
+#endif

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h

@@ -0,0 +1,72 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OVERLAPPING_PAIR_H
+#define B3_OVERLAPPING_PAIR_H
+
+#include "Bullet3Common/shared/b3Int4.h"
+
+#define B3_NEW_PAIR_MARKER -1
+#define B3_REMOVED_PAIR_MARKER -2
+
+typedef b3Int4 b3BroadphasePair;
+
+inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
+{
+	b3Int4 pair;
+
+	if (xx < yy)
+    { 
+        pair.x = xx; 
+        pair.y = yy;
+    }
+    else 
+    { 
+		pair.x = yy;
+        pair.y = xx;
+    }
+	pair.z = B3_NEW_PAIR_MARKER;
+	pair.w = B3_NEW_PAIR_MARKER;
+	return pair;
+}
+
+/*struct b3BroadphasePair : public b3Int4
+{
+	explicit b3BroadphasePair(){}
+	
+};
+*/
+
+class b3BroadphasePairSortPredicate
+{
+	public:
+
+		bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const
+		{
+			const int uidA0 = a.x;
+			const int uidB0 = b.x;
+			const int uidA1 = a.y;
+			const int uidB1 = b.y;
+			return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); 
+		}
+};
+
+B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b) 
+{
+	 return (a.x == b.x ) && (a.y == b.y );
+}
+
+#endif //B3_OVERLAPPING_PAIR_H
+

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

@@ -0,0 +1,638 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "b3OverlappingPairCache.h"
+
+//#include "b3Dispatcher.h"
+//#include "b3CollisionAlgorithm.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+
+#include <stdio.h>
+
+int	b3g_overlappingPairs = 0;
+int b3g_removePairs =0;
+int b3g_addedPairs =0;
+int b3g_findPairs =0;
+
+
+
+
+b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
+	m_overlapFilterCallback(0)
+//,	m_blockedForChanges(false)
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+
+
+
+b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
+{
+}
+
+
+
+void	b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
+{
+/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+		}
+	}
+	*/
+
+}
+
+
+
+
+void	b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
+{
+
+	class	CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache*	m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
+			:m_cleanProxy(cleanProxy),
+			m_pairCache(pairCache),
+			m_dispatcher(dispatcher)
+		{
+		}
+		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+			}
+			return false;
+		}
+		
+	};
+
+	CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+
+
+void	b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
+{
+
+	class	RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			:m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+				(pair.y == m_obsoleteProxy));
+		}
+		
+	};
+
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+
+
+
+
+b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
+{
+	b3g_findPairs++;
+	if(proxy0 >proxy1) 
+		b3Swap(proxy0,proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+	if (hash >= m_hashTable.size())
+	{
+		return NULL;
+	}
+
+	int index = m_hashTable[hash];
+	while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == B3_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	b3Assert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void	b3HashedOverlappingPairCache::growTables()
+{
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+
+		int i;
+
+		for (i= 0; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = B3_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = B3_NULL_PAIR;
+		}
+
+		for(i=0;i<curHashtableSize;i++)
+		{
+	
+			const b3BroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.x;
+			int proxyId2 = pair.y;
+			/*if (proxyId1 > proxyId2) 
+				b3Swap(proxyId1, proxyId2);*/
+			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+
+
+	}
+}
+
+b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
+{
+	if(proxy0>proxy1) 
+		b3Swap(proxy0,proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+
+
+	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+	/*for(int i=0;i<m_overlappingPairArray.size();++i)
+		{
+		if(	(m_overlappingPairArray[i].m_pProxy0==proxy0)&&
+			(m_overlappingPairArray[i].m_pProxy1==proxy1))
+			{
+			printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
+			internalFindPair(proxy0, proxy1, hash);
+			}
+		}*/
+	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
+	pair = &m_overlappingPairArray.expandNonInitializing();
+
+	//this is where we add an actual pair, so also call the 'ghost'
+//	if (m_ghostPairCallback)
+//		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	}
+	
+	*pair = b3MakeBroadphasePair(proxy0,proxy1);
+	
+//	pair->m_pProxy0 = proxy0;
+//	pair->m_pProxy1 = proxy1;
+	//pair->m_algorithm = 0;
+	//pair->m_internalTmpValue = 0;
+	
+
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = count;
+
+	return pair;
+}
+
+
+
+void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher)
+{
+	b3g_removePairs++;
+	if(proxy0>proxy1) 
+		b3Swap(proxy0,proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+
+	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	cleanOverlappingPair(*pair,dispatcher);
+
+	
+
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
+	b3Assert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int index = m_hashTable[hash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	int previous = B3_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	//if (m_ghostPairCallback)
+	//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return 0;
+	}
+
+	// Remove the last pair from the hash table.
+	const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+		/* missing swap here too, Nat. */ 
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1));
+
+	index = m_hashTable[lastHash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	previous = B3_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return 0;
+}
+//#include <stdio.h>
+
+void	b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
+{
+
+	int i;
+
+//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->x,pair->y,dispatcher);
+
+			b3g_overlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+
+
+void	b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	///need to keep hashmap in sync with pair address, so rebuild all
+	b3BroadphasePairArray tmpPairs;
+	int i;
+	for (i=0;i<m_overlappingPairArray.size();i++)
+	{
+		tmpPairs.push_back(m_overlappingPairArray[i]);
+	}
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		removeOverlappingPair(tmpPairs[i].x,tmpPairs[i].y,dispatcher);
+	}
+	
+	for (i = 0; i < m_next.size(); i++)
+	{
+		m_next[i] = B3_NULL_PAIR;
+	}
+
+	tmpPairs.quickSort(b3BroadphasePairSortPredicate());
+
+	for (i=0;i<tmpPairs.size();i++)
+	{
+		addOverlappingPair(tmpPairs[i].x ,tmpPairs[i].y);
+	}
+
+	
+}
+
+
+void*	b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher )
+{
+	if (!hasDeferredRemoval())
+	{
+		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1);
+		
+
+		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+		if (findIndex < m_overlappingPairArray.size())
+		{
+			b3g_overlappingPairs--;
+			b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
+			
+			cleanOverlappingPair(pair,dispatcher);
+			//if (m_ghostPairCallback)
+			//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+			
+			m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+			m_overlappingPairArray.pop_back();
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+
+
+
+
+
+
+
+b3BroadphasePair*	b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
+{
+	//don't add overlap with own
+	b3Assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+	
+	b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
+	*pair = b3MakeBroadphasePair(proxy0,proxy1);
+	
+	
+	b3g_overlappingPairs++;
+	b3g_addedPairs++;
+	
+//	if (m_ghostPairCallback)
+//		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
+	return pair;
+	
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+ b3BroadphasePair*	b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0,proxy1))
+		return 0;
+
+	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//b3Assert(it != m_overlappingPairSet.end());
+		 b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+
+
+
+
+
+
+
+
+
+//#include <stdio.h>
+
+void	b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
+{
+
+	int i;
+
+	for (i=0;i<m_overlappingPairArray.size();)
+	{
+	
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			cleanOverlappingPair(*pair,dispatcher);
+			pair->x = -1;
+			pair->y = -1;
+			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.pop_back();
+			b3g_overlappingPairs--;
+		} else
+		{
+			i++;
+		}
+	}
+}
+
+
+
+
+b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
+	m_blockedForChanges(false),
+	m_hasDeferredRemoval(true),
+	m_overlapFilterCallback(0)
+
+{
+	int initialAllocatedSize= 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
+{
+}
+
+void	b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
+{
+/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+			b3g_removePairs--;
+		}
+	}
+	*/
+}
+
+
+void	b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
+{
+
+	class	CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache*	m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
+			:m_cleanProxy(cleanProxy),
+			m_pairCache(pairCache),
+			m_dispatcher(dispatcher)
+		{
+		}
+		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+			}
+			return false;
+		}
+		
+	};
+
+	CleanPairCallback cleanPairs(proxy,this,dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs,dispatcher);
+
+}
+
+
+void	b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
+{
+
+	class	RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			:m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+				(pair.y == m_obsoleteProxy));
+		}
+		
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback,dispatcher);
+}
+
+void	b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	//should already be sorted
+}
+

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h

@@ -0,0 +1,474 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OVERLAPPING_PAIR_CACHE_H
+#define B3_OVERLAPPING_PAIR_CACHE_H
+
+#include "Bullet3Common/shared/b3Int2.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+class b3Dispatcher;
+#include "b3OverlappingPair.h"
+
+
+
+typedef b3AlignedObjectArray<b3BroadphasePair>	b3BroadphasePairArray;
+
+struct	b3OverlapCallback
+{
+	virtual ~b3OverlapCallback()
+	{}
+	//return true for deletion of the pair
+	virtual bool	processOverlap(b3BroadphasePair& pair) = 0;
+
+};
+
+struct b3OverlapFilterCallback
+{
+	virtual ~b3OverlapFilterCallback()
+	{}
+	// return true when pairs need collision
+	virtual bool	needBroadphaseCollision(int proxy0,int proxy1) const = 0;
+};
+
+
+
+
+
+
+
+extern int b3g_removePairs;
+extern int b3g_addedPairs;
+extern int b3g_findPairs;
+
+const int B3_NULL_PAIR=0xffffffff;
+
+///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
+///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
+class b3OverlappingPairCache 
+{
+public:
+	virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
+
+	virtual b3BroadphasePair*	getOverlappingPairArrayPtr() = 0;
+	
+	virtual const b3BroadphasePair*	getOverlappingPairArrayPtr() const = 0;
+
+	virtual b3BroadphasePairArray&	getOverlappingPairArray() = 0;
+
+	virtual	void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0;
+
+	virtual int getNumOverlappingPairs() const = 0;
+
+	virtual void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0;
+
+	virtual	void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
+
+	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0;
+
+	virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
+
+	virtual bool	hasDeferredRemoval() = 0;
+
+	//virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
+
+	virtual b3BroadphasePair* 	addOverlappingPair(int proxy0,int proxy1)=0;
+	virtual void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0;
+	virtual void	removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0;
+
+	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
+
+
+};
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+class b3HashedOverlappingPairCache : public b3OverlappingPairCache
+{
+	b3BroadphasePairArray	m_overlappingPairArray;
+	b3OverlapFilterCallback* m_overlapFilterCallback;
+//	bool		m_blockedForChanges;
+
+
+public:
+	b3HashedOverlappingPairCache();
+	virtual ~b3HashedOverlappingPairCache();
+
+	
+	virtual void	removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
+
+	virtual void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
+	
+	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+		bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+		
+		return collides;
+	}
+
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	virtual b3BroadphasePair* 	addOverlappingPair(int proxy0,int proxy1)
+	{
+		b3g_addedPairs++;
+
+		if (!needsBroadphaseCollision(proxy0,proxy1))
+			return 0;
+
+		return internalAddPair(proxy0,proxy1);
+	}
+
+	
+
+	void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
+
+	
+	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
+
+	virtual b3BroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const b3BroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	b3BroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const b3BroadphasePairArray&	getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
+
+
+
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+//	b3BroadphasePair* GetPairs() { return m_pairs; }
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int	getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+private:
+	
+	b3BroadphasePair* 	internalAddPair(int proxy0,int proxy1);
+
+	void	growTables();
+
+	B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
+	{	
+		return pair.x == proxyId1 && pair.y  == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+	{
+		int key = (proxyId2 << 16) | proxyId1;
+		key = ~key + (key << 15);
+		key = key ^ (key >> 12);
+		key = key + (key << 2);
+		key = key ^ (key >> 4);
+		key = key * 2057;
+		key = key ^ (key >> 16);
+		return key;
+	}
+	*/
+
+
+	
+	B3_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	{
+		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^=  (key >> 10);
+		key +=  (key << 3);
+		key ^=  (key >> 6);
+		key += ~(key << 11);
+		key ^=  (key >> 16);
+		return static_cast<unsigned int>(key);
+	}
+	
+
+
+
+
+	B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
+	{
+		int proxyId1 = proxy0;
+		int proxyId2 = proxy1;
+		#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+		if (proxyId1 > proxyId2) 
+			b3Swap(proxyId1, proxyId2);
+		#endif
+
+		int index = m_hashTable[hash];
+		
+		while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if ( index == B3_NULL_PAIR )
+		{
+			return NULL;
+		}
+
+		b3Assert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	virtual bool	hasDeferredRemoval()
+	{
+		return false;
+	}
+
+/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+	{
+		m_ghostPairCallback = ghostPairCallback;
+	}
+	*/
+
+	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher);
+	
+
+protected:
+	
+	b3AlignedObjectArray<int>	m_hashTable;
+	b3AlignedObjectArray<int>	m_next;
+//	b3OverlappingPairCallback*	m_ghostPairCallback;
+	
+};
+
+
+
+
+///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
+class	b3SortedOverlappingPairCache : public b3OverlappingPairCache
+{
+	protected:
+		//avoid brute-force finding all the time
+		b3BroadphasePairArray	m_overlappingPairArray;
+
+		//during the dispatch, check that user doesn't destroy/create proxy
+		bool		m_blockedForChanges;
+
+		///by default, do the removal during the pair traversal
+		bool		m_hasDeferredRemoval;
+		
+		//if set, use the callback instead of the built in filter in needBroadphaseCollision
+		b3OverlapFilterCallback* m_overlapFilterCallback;
+
+//		b3OverlappingPairCallback*	m_ghostPairCallback;
+
+	public:
+			
+		b3SortedOverlappingPairCache();	
+		virtual ~b3SortedOverlappingPairCache();
+
+		virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
+
+		void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
+
+		void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
+		
+		b3BroadphasePair*	addOverlappingPair(int proxy0,int proxy1);
+
+		b3BroadphasePair*	findPair(int proxy0,int proxy1);
+			
+		
+		void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
+
+		virtual void	removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
+
+
+		inline bool needsBroadphaseCollision(int proxy0,int proxy1) const
+		{
+			if (m_overlapFilterCallback)
+				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+
+			bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+			//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+			
+			return collides;
+		}
+		
+		b3BroadphasePairArray&	getOverlappingPairArray()
+		{
+			return m_overlappingPairArray;
+		}
+
+		const b3BroadphasePairArray&	getOverlappingPairArray() const
+		{
+			return m_overlappingPairArray;
+		}
+
+		
+
+
+		b3BroadphasePair*	getOverlappingPairArrayPtr()
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		const b3BroadphasePair*	getOverlappingPairArrayPtr() const
+		{
+			return &m_overlappingPairArray[0];
+		}
+
+		int	getNumOverlappingPairs() const
+		{
+			return m_overlappingPairArray.size();
+		}
+		
+		b3OverlapFilterCallback* getOverlapFilterCallback()
+		{
+			return m_overlapFilterCallback;
+		}
+
+		void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+		{
+			m_overlapFilterCallback = callback;
+		}
+
+		virtual bool	hasDeferredRemoval()
+		{
+			return m_hasDeferredRemoval;
+		}
+
+/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+		{
+			m_ghostPairCallback = ghostPairCallback;
+		}
+		*/
+		virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher);
+		
+
+};
+
+
+
+///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
+class b3NullPairCache : public b3OverlappingPairCache
+{
+
+	b3BroadphasePairArray	m_overlappingPairArray;
+
+public:
+
+	virtual b3BroadphasePair*	getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+	const b3BroadphasePair*	getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+	b3BroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+	
+	virtual	void	cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/)
+	{
+
+	}
+
+	virtual int getNumOverlappingPairs() const
+	{
+		return 0;
+	}
+
+	virtual void	cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/)
+	{
+
+	}
+
+	virtual	void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
+	{
+	}
+
+	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual bool	hasDeferredRemoval()
+	{
+		return true;
+	}
+
+//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
+//	{
+//
+//	}
+
+	virtual b3BroadphasePair*	addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual void*	removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/)
+	{
+		return 0;
+	}
+
+	virtual void	removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)
+	{
+	}
+	
+	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher)
+	{
+        (void) dispatcher;
+	}
+
+
+};
+
+
+#endif //B3_OVERLAPPING_PAIR_CACHE_H
+
+

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h

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

thirdparty/bullet/src/Bullet3Collision/CMakeLists.txt

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT4_H
+#define B3_CONTACT4_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int getBodyA()const {return abs(m_bodyAPtrAndSignBit);}
+	int getBodyB()const {return abs(m_bodyBPtrAndSignBit);}
+	bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;}
+	bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;}
+	//	todo. make it safer
+	int& getBatchIdx() { return m_batchIdx; }
+	const int& getBatchIdx() const { return m_batchIdx; }
+	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); }
+	void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
+	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); }
+	void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
+
+	//float& getNPoints() { return m_worldNormal[3]; }
+	int getNPoints() const { return (int) m_worldNormalOnB.w; }
+
+	float getPenetration(int idx) const { return m_worldPosB[idx].w; }
+
+	bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
+};
+
+#endif //B3_CONTACT4_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h

@@ -0,0 +1,30 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_RIGID_BODY_CL
+#define B3_RIGID_BODY_CL
+
+#include "Bullet3Common/b3Scalar.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+
+inline float	b3GetInvMass(const b3RigidBodyData& body)
+{
+		return body.m_invMass;
+}
+
+
+#endif//B3_RIGID_BODY_CL

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h

@@ -0,0 +1,20 @@
+
+#ifndef B3_BVH_SUBTREE_INFO_DATA_H
+#define B3_BVH_SUBTREE_INFO_DATA_H
+
+typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
+
+struct b3BvhSubtreeInfoData
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int			m_rootNodeIndex;
+	//4 bytes
+	int			m_subtreeSize;
+	int			m_padding[3];
+};
+
+#endif //B3_BVH_SUBTREE_INFO_DATA_H
+

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h

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

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h

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

thirdparty/bullet/src/Bullet3Collision/premake4.lua

@@ -0,0 +1,13 @@
+	project "Bullet3Collision"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

thirdparty/bullet/src/Bullet3Common/CMakeLists.txt

@@ -0,0 +1,63 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Common_SRCS
+	b3AlignedAllocator.cpp
+	b3Vector3.cpp
+	b3Logging.cpp
+)
+
+SET(Bullet3Common_HDRS
+	b3AlignedAllocator.h
+	b3AlignedObjectArray.h
+	b3CommandLineArgs.h
+	b3HashMap.h
+	b3Logging.h
+	b3Matrix3x3.h
+	b3MinMax.h
+	b3PoolAllocator.h
+	b3QuadWord.h
+	b3Quaternion.h
+	b3Random.h
+	b3Scalar.h
+	b3StackAlloc.h
+	b3Transform.h
+	b3TransformUtil.h
+	b3Vector3.h
+	shared/b3Float4
+	shared/b3Int2.h
+	shared/b3Int4.h
+	shared/b3Mat3x3.h
+	shared/b3PlatformDefinitions
+	shared/b3Quat.h
+)
+
+ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common
+					RUNTIME DESTINATION bin
+					LIBRARY DESTINATION lib${LIB_SUFFIX}
+					ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h"  PATTERN
+".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}")
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.cpp

@@ -0,0 +1,181 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b3AlignedAllocator.h"
+
+int b3g_numAlignedAllocs = 0;
+int b3g_numAlignedFree = 0;
+int b3g_totalBytesAlignedAllocs = 0;//detect memory leaks
+
+static void *b3AllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void b3FreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static b3AllocFunc* b3s_allocFunc = b3AllocDefault;
+static b3FreeFunc* b3s_freeFunc = b3FreeDefault;
+
+
+
+#if defined (B3_HAS_ALIGNED_ALLOCATOR)
+#include <malloc.h>
+static void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void b3AlignedFreeDefault(void *ptr)
+{
+	_aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return memalign(alignment, size);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+#else
+
+
+
+
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+  void *ret;
+  char *real;
+  real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment-1));
+  if (real) {
+	ret = b3AlignPointer(real + sizeof(void *),alignment);
+    *((void **)(ret)-1) = (void *)(real);
+  } else {
+    ret = (void *)(real);
+  }
+  return (ret);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+  void* real;
+
+  if (ptr) {
+    real = *((void **)(ptr)-1);
+    b3s_freeFunc(real);
+  }
+}
+#endif
+
+
+static b3AlignedAllocFunc* b3s_alignedAllocFunc = b3AlignedAllocDefault;
+static b3AlignedFreeFunc* b3s_alignedFreeFunc = b3AlignedFreeDefault;
+
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
+{
+  b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
+  b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
+}
+
+void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
+{
+  b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
+  b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
+}
+
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void*   b3AlignedAllocInternal  (size_t size, int alignment,int line,char* filename)
+{
+ void *ret;
+ char *real;
+
+ b3g_totalBytesAlignedAllocs += size;
+ b3g_numAlignedAllocs++;
+
+ 
+ real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1));
+ if (real) {
+   ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
+   *((void **)(ret)-1) = (void *)(real);
+       *((int*)(ret)-2) = size;
+
+ } else {
+   ret = (void *)(real);//??
+ }
+
+ b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size);
+
+ int* ptr = (int*)ret;
+ *ptr = 12;
+ return (ret);
+}
+
+void    b3AlignedFreeInternal   (void* ptr,int line,char* filename)
+{
+
+ void* real;
+ b3g_numAlignedFree++;
+
+ if (ptr) {
+   real = *((void **)(ptr)-1);
+       int size = *((int*)(ptr)-2);
+       b3g_totalBytesAlignedAllocs -= size;
+
+	   b3Printf("free #%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedFree,real, filename,line,size);
+
+   b3s_freeFunc(real);
+ } else
+ {
+	 b3Printf("NULL ptr\n");
+ }
+}
+
+#else //B3_DEBUG_MEMORY_ALLOCATIONS
+
+void*	b3AlignedAllocInternal	(size_t size, int alignment)
+{
+	b3g_numAlignedAllocs++;
+	void* ptr;
+	ptr = b3s_alignedAllocFunc(size, alignment);
+//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
+	return ptr;
+}
+
+void	b3AlignedFreeInternal	(void* ptr)
+{
+	if (!ptr)
+	{
+		return;
+	}
+
+	b3g_numAlignedFree++;
+//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
+	b3s_alignedFreeFunc(ptr);
+}
+
+#endif //B3_DEBUG_MEMORY_ALLOCATIONS
+

thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.h

@@ -0,0 +1,107 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_ALIGNED_ALLOCATOR
+#define B3_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "b3Scalar.h"
+//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+
+#define b3AlignedAlloc(a,b) \
+		b3AlignedAllocInternal(a,b,__LINE__,__FILE__)
+
+#define b3AlignedFree(ptr) \
+		b3AlignedFreeInternal(ptr,__LINE__,__FILE__)
+
+void*	b3AlignedAllocInternal	(size_t size, int alignment,int line,char* filename);
+
+void	b3AlignedFreeInternal	(void* ptr,int line,char* filename);
+
+#else
+	void*	b3AlignedAllocInternal	(size_t size, int alignment);
+	void	b3AlignedFreeInternal	(void* ptr);
+
+	#define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment)
+	#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
+
+#endif
+typedef int	btSizeType;
+
+typedef void *(b3AlignedAllocFunc)(size_t size, int alignment);
+typedef void (b3AlignedFreeFunc)(void *memblock);
+typedef void *(b3AllocFunc)(size_t size);
+typedef void (b3FreeFunc)(void *memblock);
+
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
+void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc);
+///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc);
+
+
+///The b3AlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
+template < typename T , unsigned Alignment >
+class b3AlignedAllocator {
+	
+	typedef b3AlignedAllocator< T , Alignment > self_type;
+	
+public:
+
+	//just going down a list:
+	b3AlignedAllocator() {}
+	/*
+	b3AlignedAllocator( const self_type & ) {}
+	*/
+
+	template < typename Other >
+	b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
+
+	typedef const T*         const_pointer;
+	typedef const T&         const_reference;
+	typedef T*               pointer;
+	typedef T&               reference;
+	typedef T                value_type;
+
+	pointer       address   ( reference        ref ) const                           { return &ref; }
+	const_pointer address   ( const_reference  ref ) const                           { return &ref; }
+	pointer       allocate  ( btSizeType        n   , const_pointer *      hint = 0 ) {
+		(void)hint;
+		return reinterpret_cast< pointer >(b3AlignedAlloc( sizeof(value_type) * n , Alignment ));
+	}
+	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
+	void          deallocate( pointer          ptr ) {
+		b3AlignedFree( reinterpret_cast< void * >( ptr ) );
+	}
+	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
+	
+
+	template < typename O > struct rebind {
+		typedef b3AlignedAllocator< O , Alignment > other;
+	};
+	template < typename O >
+	self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
+
+	friend bool operator==( const self_type & , const self_type & ) { return true; }
+};
+
+
+
+#endif //B3_ALIGNED_ALLOCATOR
+

thirdparty/bullet/src/Bullet3Common/b3AlignedObjectArray.h

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

thirdparty/bullet/src/Bullet3Common/b3CommandLineArgs.h

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

thirdparty/bullet/src/Bullet3Common/b3FileUtils.h

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

thirdparty/bullet/src/Bullet3Common/b3HashMap.h

@@ -0,0 +1,466 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef B3_HASH_MAP_H
+#define B3_HASH_MAP_H
+
+#include "b3AlignedObjectArray.h"
+
+
+#include <string>
+
+///very basic hashable string implementation, compatible with b3HashMap
+struct b3HashString
+{
+	std::string m_string;
+	unsigned int	m_hash;
+
+	B3_FORCE_INLINE	unsigned int getHash()const
+	{
+		return m_hash;
+	}
+
+
+	b3HashString(const char* name)
+		:m_string(name)
+	{
+
+		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
+		static const unsigned int  InitialFNV = 2166136261u;
+		static const unsigned int FNVMultiple = 16777619u;
+
+		/* Fowler / Noll / Vo (FNV) Hash */
+		unsigned int hash = InitialFNV;
+		int len = m_string.length();
+		for(int i = 0; i<len; i++)
+		{
+			hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
+			hash = hash * FNVMultiple;  /* multiply by the magic number */
+		}
+		m_hash = hash;
+	}
+
+	int portableStringCompare(const char* src,	const char* dst) const
+	{
+			int ret = 0 ;
+
+			while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
+					++src, ++dst;
+
+			if ( ret < 0 )
+					ret = -1 ;
+			else if ( ret > 0 )
+					ret = 1 ;
+
+			return( ret );
+	}
+
+	bool equals(const b3HashString& other) const
+	{
+		return (m_string == other.m_string);
+	}
+
+};
+
+
+const int B3_HASH_NULL=0xffffffff;
+
+
+class b3HashInt
+{
+	int	m_uid;
+public:
+	b3HashInt(int uid)	:m_uid(uid)
+	{
+	}
+
+	int	getUid1() const
+	{
+		return m_uid;
+	}
+
+	void	setUid1(int uid)
+	{
+		m_uid = uid;
+	}
+
+	bool equals(const b3HashInt& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE	unsigned int getHash()const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		return key;
+	}
+};
+
+
+
+class b3HashPtr
+{
+
+	union
+	{
+		const void*	m_pointer;
+		int	m_hashValues[2];
+	};
+
+public:
+
+	b3HashPtr(const void* ptr)
+		:m_pointer(ptr)
+	{
+	}
+
+	const void*	getPointer() const
+	{
+		return m_pointer;
+	}
+
+	bool equals(const b3HashPtr& other) const
+	{
+		return getPointer() == other.getPointer();
+	}
+
+	//to our success
+	B3_FORCE_INLINE	unsigned int getHash()const
+	{
+		const bool VOID_IS_8 = ((sizeof(void*)==8));
+		
+		int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
+	
+		// Thomas Wang's hash
+		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		return key;
+	}
+
+	
+};
+
+
+template <class Value>
+class b3HashKeyPtr
+{
+        int     m_uid;
+public:
+
+        b3HashKeyPtr(int uid)    :m_uid(uid)
+        {
+        }
+
+        int     getUid1() const
+        {
+                return m_uid;
+        }
+
+        bool equals(const b3HashKeyPtr<Value>& other) const
+        {
+                return getUid1() == other.getUid1();
+        }
+
+        //to our success
+        B3_FORCE_INLINE       unsigned int getHash()const
+        {
+                int key = m_uid;
+                // Thomas Wang's hash
+                key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+                return key;
+        }
+
+        
+};
+
+
+template <class Value>
+class b3HashKey
+{
+	int	m_uid;
+public:
+
+	b3HashKey(int uid)	:m_uid(uid)
+	{
+	}
+
+	int	getUid1() const
+	{
+		return m_uid;
+	}
+
+	bool equals(const b3HashKey<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE	unsigned int getHash()const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		return key;
+	}
+};
+
+
+///The b3HashMap template class implements a generic and lightweight hashmap.
+///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
+template <class Key, class Value>
+class b3HashMap
+{
+
+protected:
+	b3AlignedObjectArray<int>		m_hashTable;
+	b3AlignedObjectArray<int>		m_next;
+	
+	b3AlignedObjectArray<Value>		m_valueArray;
+	b3AlignedObjectArray<Key>		m_keyArray;
+
+	void	growTables(const Key& /*key*/)
+	{
+		int newCapacity = m_valueArray.capacity();
+
+		if (m_hashTable.size() < newCapacity)
+		{
+			//grow hashtable and next table
+			int curHashtableSize = m_hashTable.size();
+
+			m_hashTable.resize(newCapacity);
+			m_next.resize(newCapacity);
+
+			int i;
+
+			for (i= 0; i < newCapacity; ++i)
+			{
+				m_hashTable[i] = B3_HASH_NULL;
+			}
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_next[i] = B3_HASH_NULL;
+			}
+
+			for(i=0;i<curHashtableSize;i++)
+			{
+				//const Value& value = m_valueArray[i];
+				//const Key& key = m_keyArray[i];
+
+				int	hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1);	// New hash value with new mask
+				m_next[i] = m_hashTable[hashValue];
+				m_hashTable[hashValue] = i;
+			}
+
+
+		}
+	}
+
+	public:
+
+	void insert(const Key& key, const Value& value) {
+		int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+		//replace value if the key is already there
+		int index = findIndex(key);
+		if (index != B3_HASH_NULL)
+		{
+			m_valueArray[index]=value;
+			return;
+		}
+
+		int count = m_valueArray.size();
+		int oldCapacity = m_valueArray.capacity();
+		m_valueArray.push_back(value);
+		m_keyArray.push_back(key);
+
+		int newCapacity = m_valueArray.capacity();
+		if (oldCapacity < newCapacity)
+		{
+			growTables(key);
+			//hash with new capacity
+			hash = key.getHash() & (m_valueArray.capacity()-1);
+		}
+		m_next[count] = m_hashTable[hash];
+		m_hashTable[hash] = count;
+	}
+
+	void remove(const Key& key) {
+
+		int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+		int pairIndex = findIndex(key);
+		
+		if (pairIndex ==B3_HASH_NULL)
+		{
+			return;
+		}
+
+		// Remove the pair from the hash table.
+		int index = m_hashTable[hash];
+		b3Assert(index != B3_HASH_NULL);
+
+		int previous = B3_HASH_NULL;
+		while (index != pairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == pairIndex);
+			m_next[previous] = m_next[pairIndex];
+		}
+		else
+		{
+			m_hashTable[hash] = m_next[pairIndex];
+		}
+
+		// We now move the last pair into spot of the
+		// pair being removed. We need to fix the hash
+		// table indices to support the move.
+
+		int lastPairIndex = m_valueArray.size() - 1;
+
+		// If the removed pair is the last pair, we are done.
+		if (lastPairIndex == pairIndex)
+		{
+			m_valueArray.pop_back();
+			m_keyArray.pop_back();
+			return;
+		}
+
+		// Remove the last pair from the hash table.
+		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
+
+		index = m_hashTable[lastHash];
+		b3Assert(index != B3_HASH_NULL);
+
+		previous = B3_HASH_NULL;
+		while (index != lastPairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == lastPairIndex);
+			m_next[previous] = m_next[lastPairIndex];
+		}
+		else
+		{
+			m_hashTable[lastHash] = m_next[lastPairIndex];
+		}
+
+		// Copy the last pair into the remove pair's spot.
+		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+		m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
+
+		// Insert the last pair into the hash table
+		m_next[pairIndex] = m_hashTable[lastHash];
+		m_hashTable[lastHash] = pairIndex;
+
+		m_valueArray.pop_back();
+		m_keyArray.pop_back();
+
+	}
+
+
+	int size() const
+	{
+		return m_valueArray.size();
+	}
+
+	const Value* getAtIndex(int index) const
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Value* getAtIndex(int index)
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	 Key getKeyAtIndex(int index)
+    {
+        b3Assert(index < m_keyArray.size());
+        return m_keyArray[index];
+    }
+    
+    const Key getKeyAtIndex(int index) const
+    {
+        b3Assert(index < m_keyArray.size());
+        return m_keyArray[index];
+    }
+
+	Value* operator[](const Key& key) {
+		return find(key);
+	}
+
+	const Value*	find(const Key& key) const
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	Value*	find(const Key& key)
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+
+	int	findIndex(const Key& key) const
+	{
+		unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
+
+		if (hash >= (unsigned int)m_hashTable.size())
+		{
+			return B3_HASH_NULL;
+		}
+
+		int index = m_hashTable[hash];
+		while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
+		{
+			index = m_next[index];
+		}
+		return index;
+	}
+
+	void	clear()
+	{
+		m_hashTable.clear();
+		m_next.clear();
+		m_valueArray.clear();
+		m_keyArray.clear();
+	}
+
+};
+
+#endif //B3_HASH_MAP_H

thirdparty/bullet/src/Bullet3Common/b3Logging.cpp

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

thirdparty/bullet/src/Bullet3Common/b3Logging.h

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

thirdparty/bullet/src/Bullet3Common/b3Matrix3x3.h

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

thirdparty/bullet/src/Bullet3Common/b3MinMax.h

@@ -0,0 +1,71 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef B3_GEN_MINMAX_H
+#define B3_GEN_MINMAX_H
+
+#include "b3Scalar.h"
+
+template <class T>
+B3_FORCE_INLINE const T& b3Min(const T& a, const T& b) 
+{
+  return a < b ? a : b ;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Max(const T& a, const T& b) 
+{
+  return  a > b ? a : b;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub) 
+{
+	return a < lb ? lb : (ub < a ? ub : a); 
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMin(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMax(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
+#endif //B3_GEN_MINMAX_H

thirdparty/bullet/src/Bullet3Common/b3PoolAllocator.h

@@ -0,0 +1,121 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef _BT_POOL_ALLOCATOR_H
+#define _BT_POOL_ALLOCATOR_H
+
+#include "b3Scalar.h"
+#include "b3AlignedAllocator.h"
+
+///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
+class b3PoolAllocator
+{
+	int				m_elemSize;
+	int				m_maxElements;
+	int				m_freeCount;
+	void*			m_firstFree;
+	unsigned char*	m_pool;
+
+public:
+
+	b3PoolAllocator(int elemSize, int maxElements)
+		:m_elemSize(elemSize),
+		m_maxElements(maxElements)
+	{
+		m_pool = (unsigned char*) b3AlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
+
+		unsigned char* p = m_pool;
+        m_firstFree = p;
+        m_freeCount = m_maxElements;
+        int count = m_maxElements;
+        while (--count) {
+            *(void**)p = (p + m_elemSize);
+            p += m_elemSize;
+        }
+        *(void**)p = 0;
+    }
+
+	~b3PoolAllocator()
+	{
+		b3AlignedFree( m_pool);
+	}
+
+	int	getFreeCount() const
+	{
+		return m_freeCount;
+	}
+
+	int getUsedCount() const
+	{
+		return m_maxElements - m_freeCount;
+	}
+
+	int getMaxCount() const
+	{
+		return m_maxElements;
+	}
+
+	void*	allocate(int size)
+	{
+		// release mode fix
+		(void)size;
+		b3Assert(!size || size<=m_elemSize);
+		b3Assert(m_freeCount>0);
+        void* result = m_firstFree;
+        m_firstFree = *(void**)m_firstFree;
+        --m_freeCount;
+        return result;
+	}
+
+	bool validPtr(void* ptr)
+	{
+		if (ptr) {
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	void	freeMemory(void* ptr)
+	{
+		 if (ptr) {
+            b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+            *(void**)ptr = m_firstFree;
+            m_firstFree = ptr;
+            ++m_freeCount;
+        }
+	}
+
+	int	getElementSize() const
+	{
+		return m_elemSize;
+	}
+
+	unsigned char*	getPoolAddress()
+	{
+		return m_pool;
+	}
+
+	const unsigned char*	getPoolAddress() const
+	{
+		return m_pool;
+	}
+
+};
+
+#endif //_BT_POOL_ALLOCATOR_H

thirdparty/bullet/src/Bullet3Common/b3QuadWord.h

@@ -0,0 +1,245 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef B3_SIMD_QUADWORD_H
+#define B3_SIMD_QUADWORD_H
+
+#include "b3Scalar.h"
+#include "b3MinMax.h"
+
+
+
+
+
+#if defined (__CELLOS_LV2) && defined (__SPU__)
+#include <altivec.h>
+#endif
+
+/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion. 
+ * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
+ */
+#ifndef USE_LIBSPE2
+B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord
+#else
+class b3QuadWord
+#endif
+{
+protected:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		b3Scalar	m_floats[4];
+	};
+public:
+	vec_float4	get128() const
+	{
+		return mVec128;
+	}
+
+#else //__CELLOS_LV2__ __SPU__
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON) 
+public:
+	union {
+		b3SimdFloat4 mVec128;
+		b3Scalar	m_floats[4];
+		struct {b3Scalar x,y,z,w;};
+	};
+public:
+	B3_FORCE_INLINE	b3SimdFloat4	get128() const
+	{
+		return mVec128;
+	}
+	B3_FORCE_INLINE	void	set128(b3SimdFloat4 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+public:
+	union
+	{
+		b3Scalar	m_floats[4];
+		struct {b3Scalar x,y,z,w;};
+	};
+#endif // B3_USE_SSE
+
+#endif //__CELLOS_LV2__ __SPU__
+
+	public:
+  
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+	// Set Vector 
+	B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
+	{
+		mVec128 = vec;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
+	{
+		mVec128 = rhs.mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3QuadWord& 
+	operator=(const b3QuadWord& v) 
+	{
+		mVec128 = v.mVec128;
+		
+		return *this;
+	}
+	
+#endif
+
+  /**@brief Return the x value */
+		B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+  /**@brief Set the x value */
+		B3_FORCE_INLINE void	setX(b3Scalar _x) { m_floats[0] = _x;};
+  /**@brief Set the y value */
+		B3_FORCE_INLINE void	setY(b3Scalar _y) { m_floats[1] = _y;};
+  /**@brief Set the z value */
+		B3_FORCE_INLINE void	setZ(b3Scalar _z) { m_floats[2] = _z;};
+  /**@brief Set the w value */
+		B3_FORCE_INLINE void	setW(b3Scalar _w) { m_floats[3] = _w;};
+  /**@brief Return the x value */
+
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	B3_FORCE_INLINE	operator       b3Scalar *()       { return &m_floats[0]; }
+	B3_FORCE_INLINE	operator const b3Scalar *() const { return &m_floats[0]; }
+
+	B3_FORCE_INLINE	bool	operator==(const b3QuadWord& other) const
+	{
+#ifdef B3_USE_SSE
+        return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else 
+		return ((m_floats[3]==other.m_floats[3]) && 
+                (m_floats[2]==other.m_floats[2]) && 
+                (m_floats[1]==other.m_floats[1]) && 
+                (m_floats[0]==other.m_floats[0]));
+#endif
+	}
+
+	B3_FORCE_INLINE	bool	operator!=(const b3QuadWord& other) const
+	{
+		return !(*this == other);
+	}
+
+  /**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		B3_FORCE_INLINE void 	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+		{
+			m_floats[0]=_x;
+			m_floats[1]=_y;
+			m_floats[2]=_z;
+			m_floats[3] = 0.f;
+		}
+
+/*		void getValue(b3Scalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] = m_floats[2];
+		}
+*/
+/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		B3_FORCE_INLINE void	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
+		{
+			m_floats[0]=_x;
+			m_floats[1]=_y;
+			m_floats[2]=_z;
+			m_floats[3]=_w;
+		}
+  /**@brief No initialization constructor */
+		B3_FORCE_INLINE b3QuadWord()
+		//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
+		{
+		}
+ 
+  /**@brief Three argument constructor (zeros w)
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)		
+		{
+			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
+		}
+
+/**@brief Initializing constructor
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w) 
+		{
+			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
+		}
+
+  /**@brief Set each element to the max of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+		B3_FORCE_INLINE void	setMax(const b3QuadWord& other)
+		{
+        #ifdef B3_USE_SSE
+            mVec128 = _mm_max_ps(mVec128, other.mVec128);
+        #elif defined(B3_USE_NEON)
+            mVec128 = vmaxq_f32(mVec128, other.mVec128);
+        #else
+        	b3SetMax(m_floats[0], other.m_floats[0]);
+			b3SetMax(m_floats[1], other.m_floats[1]);
+			b3SetMax(m_floats[2], other.m_floats[2]);
+			b3SetMax(m_floats[3], other.m_floats[3]);
+		#endif
+        }
+  /**@brief Set each element to the min of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+		B3_FORCE_INLINE void	setMin(const b3QuadWord& other)
+		{
+        #ifdef B3_USE_SSE
+            mVec128 = _mm_min_ps(mVec128, other.mVec128);
+        #elif defined(B3_USE_NEON)
+            mVec128 = vminq_f32(mVec128, other.mVec128);
+        #else
+        	b3SetMin(m_floats[0], other.m_floats[0]);
+			b3SetMin(m_floats[1], other.m_floats[1]);
+			b3SetMin(m_floats[2], other.m_floats[2]);
+			b3SetMin(m_floats[3], other.m_floats[3]);
+		#endif
+        }
+
+
+
+};
+
+#endif //B3_SIMD_QUADWORD_H

thirdparty/bullet/src/Bullet3Common/b3Quaternion.h

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