19 files changed, 185 insertions, 123 deletions

diff --git a/modules/bullet/area_bullet.cpp b/modules/bullet/area_bullet.cpp
index 648919e612..ec78cddb6a 100644
--- a/modules/bullet/area_bullet.cpp
+++ b/modules/bullet/area_bullet.cpp
@@ -68,7 +68,8 @@ AreaBullet::AreaBullet() :
 }
 
 AreaBullet::~AreaBullet() {
-	remove_all_overlapping_instantly();
+	// Call "remove_all_overlapping_instantly();" is not necessary because the exit
+	// signal are handled by godot, so just clear the array
 }
 
 void AreaBullet::dispatch_callbacks() {
@@ -131,12 +132,13 @@ void AreaBullet::remove_all_overlapping_instantly() {
 	overlappingObjects.clear();
 }
 
-void AreaBullet::remove_overlapping_instantly(CollisionObjectBullet *p_object) {
+void AreaBullet::remove_overlapping_instantly(CollisionObjectBullet *p_object, bool p_notify) {
 	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);
+			if (p_notify)
+				call_event(supportObject, PhysicsServer::AREA_BODY_REMOVED);
 			supportObject->on_exit_area(this);
 			overlappingObjects.remove(i);
 			break;
diff --git a/modules/bullet/area_bullet.h b/modules/bullet/area_bullet.h
index 78136d574b..4104780de9 100644
--- a/modules/bullet/area_bullet.h
+++ b/modules/bullet/area_bullet.h
@@ -152,7 +152,7 @@ public:
 
 	void remove_all_overlapping_instantly();
 	// Dispatch the callbacks and removes from overlapping list
-	void remove_overlapping_instantly(CollisionObjectBullet *p_object);
+	void remove_overlapping_instantly(CollisionObjectBullet *p_object, bool p_notify);
 
 	virtual void on_collision_filters_change();
 	virtual void on_collision_checker_start() {}
diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp
index 4164450cd2..8707096038 100644
--- a/modules/bullet/btRayShape.cpp
+++ b/modules/bullet/btRayShape.cpp
@@ -54,6 +54,11 @@ void btRayShape::setLength(btScalar p_length) {
 	reload_cache();
 }
 
+void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
+
+	slipsOnSlope = p_slipsOnSlope;
+}
+
 btVector3 btRayShape::localGetSupportingVertex(const btVector3 &vec) const {
 	return localGetSupportingVertexWithoutMargin(vec) + (m_shapeAxis * m_collisionMargin);
 }
diff --git a/modules/bullet/btRayShape.h b/modules/bullet/btRayShape.h
index 99a9412dbe..a44c30db4b 100644
--- a/modules/bullet/btRayShape.h
+++ b/modules/bullet/btRayShape.h
@@ -44,6 +44,7 @@ ATTRIBUTE_ALIGNED16(class)
 btRayShape : public btConvexInternalShape {
 
 	btScalar m_length;
+	bool slipsOnSlope;
 	/// The default axis is the z
 	btVector3 m_shapeAxis;
 
@@ -59,6 +60,9 @@ public:
 	void setLength(btScalar p_length);
 	btScalar getLength() const { return m_length; }
 
+	void setSlipsOnSlope(bool p_slipOnSlope);
+	bool getSlipsOnSlope() const { return slipsOnSlope; }
+
 	const btTransform &getSupportPoint() const { return m_cacheSupportPoint; }
 	const btScalar &getScaledLength() const { return m_cacheScaledLength; }
 
diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index b646fc164d..6246a295ec 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -89,7 +89,9 @@ BulletPhysicsServer::BulletPhysicsServer() :
 		active(true),
 		active_spaces_count(0) {}
 
-BulletPhysicsServer::~BulletPhysicsServer() {}
+BulletPhysicsServer::~BulletPhysicsServer() {
+	bulletdelete(emptyShape);
+}
 
 RID BulletPhysicsServer::shape_create(ShapeType p_shape) {
 	ShapeBullet *shape = NULL;
@@ -619,11 +621,11 @@ uint32_t BulletPhysicsServer::body_get_collision_mask(RID p_body) const {
 }
 
 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");
+	// This function si not currently supported
 }
 
 uint32_t BulletPhysicsServer::body_get_user_flags(RID p_body) const {
-	WARN_PRINT("This function si not currently supported by bullet and Godot");
+	// This function si not currently supported
 	return 0;
 }
 
@@ -783,22 +785,27 @@ int BulletPhysicsServer::body_get_max_contacts_reported(RID p_body) const {
 	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");
+void BulletPhysicsServer::body_set_contacts_reported_depth_threshold(RID p_body, float p_threshold) {
+	// 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");
+	// 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");
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_omit_forces_integration(p_omit);
 }
 
 bool BulletPhysicsServer::body_is_omitting_force_integration(RID p_body) const {
-	WARN_PRINT("Not supported by bullet");
-	return false;
+	RigidBodyBullet *body = rigid_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+
+	return body->get_omit_forces_integration();
 }
 
 void BulletPhysicsServer::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
@@ -825,12 +832,12 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
 	return BulletPhysicsDirectBodyState::get_singleton(body);
 }
 
-bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) {
+bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, 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, r_result);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, r_result);
 }
 
 RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {
@@ -979,11 +986,11 @@ PhysicsServer::JointType BulletPhysicsServer::joint_get_type(RID p_joint) const
 }
 
 void BulletPhysicsServer::joint_set_solver_priority(RID p_joint, int p_priority) {
-	//WARN_PRINTS("Joint priority not supported by bullet");
+	// Joint priority not supported by bullet
 }
 
 int BulletPhysicsServer::joint_get_solver_priority(RID p_joint) const {
-	//WARN_PRINTS("Joint priority not supported by bullet");
+	// Joint priority not supported by bullet
 	return 0;
 }
 
diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h
index 764ec2387c..e931915bba 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -239,7 +239,7 @@ public:
 	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 void body_set_contacts_reported_depth_threshold(RID p_body, float p_threshold);
 	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);
@@ -253,7 +253,7 @@ public:
 	// 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, MotionResult *r_result = NULL);
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = NULL);
 
 	/* SOFT BODY API */
 
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 34aff68a4a..77f8df34cb 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -68,12 +68,10 @@ CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
 		force_shape_reset(false) {}
 
 CollisionObjectBullet::~CollisionObjectBullet() {
-	// Remove all overlapping
+	// Remove all overlapping, notify is not required since godot take care of it
 	for (int i = areasOverlapped.size() - 1; 0 <= i; --i) {
-		areasOverlapped[i]->remove_overlapping_instantly(this);
+		areasOverlapped[i]->remove_overlapping_instantly(this, /*Notify*/ false);
 	}
-	// not required
-	// areasOverlapped.clear();
 
 	destroyBulletCollisionObject();
 }
diff --git a/modules/bullet/doc_classes/BulletPhysicsDirectBodyState.xml b/modules/bullet/doc_classes/BulletPhysicsDirectBodyState.xml
index c7909c7d72..a4dc61d0bc 100644
--- a/modules/bullet/doc_classes/BulletPhysicsDirectBodyState.xml
+++ b/modules/bullet/doc_classes/BulletPhysicsDirectBodyState.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="BulletPhysicsDirectBodyState" inherits="PhysicsDirectBodyState" category="Core" version="3.0-stable">
+<class name="BulletPhysicsDirectBodyState" inherits="PhysicsDirectBodyState" category="Core" version="3.1">
 	<brief_description>
 	</brief_description>
 	<description>
diff --git a/modules/bullet/doc_classes/BulletPhysicsServer.xml b/modules/bullet/doc_classes/BulletPhysicsServer.xml
index a59abb0ebb..1486936cf4 100644
--- a/modules/bullet/doc_classes/BulletPhysicsServer.xml
+++ b/modules/bullet/doc_classes/BulletPhysicsServer.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="BulletPhysicsServer" inherits="PhysicsServer" category="Core" version="3.0-stable">
+<class name="BulletPhysicsServer" inherits="PhysicsServer" category="Core" version="3.1">
 	<brief_description>
 	</brief_description>
 	<description>
diff --git a/modules/bullet/godot_motion_state.h b/modules/bullet/godot_motion_state.h
index fe5d8418b7..fa58e86589 100644
--- a/modules/bullet/godot_motion_state.h
+++ b/modules/bullet/godot_motion_state.h
@@ -87,7 +87,7 @@ public:
 
 public:
 	/// Use this function to move kinematic body
-	/// -- or set initial transfom before body creation.
+	/// -- or set initial transform before body creation.
 	void moveBody(const btTransform &newWorldTransform) {
 		bodyKinematicWorldTransf = newWorldTransform;
 	}
diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp
index 4a511b39a7..53d0ab7e3c 100644
--- a/modules/bullet/godot_ray_world_algorithm.cpp
+++ b/modules/bullet/godot_ray_world_algorithm.cpp
@@ -100,14 +100,16 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo
 
 	if (btResult.hasHit()) {
 
-		btVector3 ray_normal(ray_transform.getOrigin() - to.getOrigin());
-		ray_normal.normalize();
 		btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
 
 		if (depth >= -RAY_STABILITY_MARGIN)
 			depth = 0;
 
-		resultOut->addContactPoint(ray_normal, btResult.m_hitPointWorld, depth);
+		if (ray_shape->getSlipsOnSlope())
+			resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
+		else {
+			resultOut->addContactPoint((ray_transform.getOrigin() - to.getOrigin()).normalize(), btResult.m_hitPointWorld, depth);
+		}
 	}
 }
 
diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp
index 8d4ca6d6a7..caa3d677dd 100644
--- a/modules/bullet/godot_result_callbacks.cpp
+++ b/modules/bullet/godot_result_callbacks.cpp
@@ -63,6 +63,9 @@ bool GodotClosestRayResultCallback::needsCollision(btBroadphaseProxy *proxy0) co
 }
 
 bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	if (count >= m_resultMax)
+		return false;
+
 	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);
@@ -70,6 +73,7 @@ bool GodotAllConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) con
 		if (m_exclude->has(gObj->get_self())) {
 			return false;
 		}
+
 		return true;
 	} else {
 		return false;
@@ -87,7 +91,7 @@ btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalCo
 	result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
 
 	++count;
-	return count < m_resultMax;
+	return 1; // not used by bullet
 }
 
 bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
@@ -98,11 +102,16 @@ bool GodotKinClosestConvexResultCallback::needsCollision(btBroadphaseProxy *prox
 		if (gObj == m_self_object) {
 			return false;
 		} else {
-			if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
+
+			// A kinematic body can't be stopped by a rigid body since the mass of kinematic body is infinite
+			if (m_infinite_inertia && !btObj->isStaticOrKinematicObject())
 				return false;
-			} else if (m_self_object->has_collision_exception(gObj)) {
+
+			if (gObj->getType() == CollisionObjectBullet::TYPE_AREA)
+				return false;
+
+			if (m_self_object->has_collision_exception(gObj))
 				return false;
-			}
 		}
 		return true;
 	} else {
@@ -176,6 +185,9 @@ btScalar GodotAllContactResultCallback::addSingleResult(btManifoldPoint &cp, con
 }
 
 bool GodotContactPairContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
+	if (m_count >= m_resultMax)
+		return false;
+
 	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);
@@ -201,7 +213,7 @@ btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint
 
 	++m_count;
 
-	return m_count < m_resultMax;
+	return 1; // Not used by bullet
 }
 
 bool GodotRestInfoContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
@@ -247,23 +259,17 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
 		m_collided = true;
 	}
 
-	return cp.getDistance();
+	return 1; // Not used by bullet
 }
 
 void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) {
 
-	if (depth < 0) {
-		// Has penetration
-		if (m_most_penetrated_distance > depth) {
-
-			bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+	if (m_penetration_distance > depth) { // Has penetration?
 
-			m_most_penetrated_distance = depth;
-			m_pointCollisionObject = (isSwapped ? m_body0Wrap : m_body1Wrap)->getCollisionObject();
-			m_other_compound_shape_index = isSwapped ? m_index1 : m_index0;
-			m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
-			m_pointWorld = isSwapped ? (pointInWorldOnB + normalOnBInWorld * depth) : pointInWorldOnB;
-			m_penetration_distance = depth;
-		}
+		bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
+		m_penetration_distance = depth;
+		m_other_compound_shape_index = isSwapped ? m_index0 : m_index1;
+		m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
+		m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB;
 	}
 }
diff --git a/modules/bullet/godot_result_callbacks.h b/modules/bullet/godot_result_callbacks.h
index e1b0b1b421..363051f24c 100644
--- a/modules/bullet/godot_result_callbacks.h
+++ b/modules/bullet/godot_result_callbacks.h
@@ -93,12 +93,12 @@ public:
 struct GodotKinClosestConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback {
 public:
 	const RigidBodyBullet *m_self_object;
-	const bool m_ignore_areas;
+	const bool m_infinite_inertia;
 
-	GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_ignore_areas) :
+	GodotKinClosestConvexResultCallback(const btVector3 &convexFromWorld, const btVector3 &convexToWorld, const RigidBodyBullet *p_self_object, bool p_infinite_inertia) :
 			btCollisionWorld::ClosestConvexResultCallback(convexFromWorld, convexToWorld),
 			m_self_object(p_self_object),
-			m_ignore_areas(p_ignore_areas) {}
+			m_infinite_inertia(p_infinite_inertia) {}
 
 	virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
 };
@@ -185,24 +185,18 @@ struct GodotDeepPenetrationContactResultCallback : public btManifoldResult {
 	btVector3 m_pointWorld;
 	btScalar m_penetration_distance;
 	int m_other_compound_shape_index;
-	const btCollisionObject *m_pointCollisionObject;
-
-	btScalar m_most_penetrated_distance;
 
 	GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) :
 			btManifoldResult(body0Wrap, body1Wrap),
-			m_pointCollisionObject(NULL),
 			m_penetration_distance(0),
-			m_other_compound_shape_index(0),
-			m_most_penetrated_distance(1e20) {}
+			m_other_compound_shape_index(0) {}
 
 	void reset() {
-		m_pointCollisionObject = NULL;
-		m_most_penetrated_distance = 1e20;
+		m_penetration_distance = 0;
 	}
 
 	bool hasHit() {
-		return m_pointCollisionObject;
+		return m_penetration_distance < 0;
 	}
 
 	virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth);
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index f96218ef46..2494063c22 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -114,10 +114,18 @@ Transform BulletPhysicsDirectBodyState::get_transform() const {
 	return body->get_transform();
 }
 
+void BulletPhysicsDirectBodyState::add_central_force(const Vector3 &p_force) {
+	body->apply_central_force(p_force);
+}
+
 void BulletPhysicsDirectBodyState::add_force(const Vector3 &p_force, const Vector3 &p_pos) {
 	body->apply_force(p_force, p_pos);
 }
 
+void BulletPhysicsDirectBodyState::add_torque(const Vector3 &p_torque) {
+	body->apply_torque(p_torque);
+}
+
 void BulletPhysicsDirectBodyState::apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) {
 	body->apply_impulse(p_pos, p_j);
 }
@@ -247,6 +255,7 @@ RigidBodyBullet::RigidBodyBullet() :
 		linearDamp(0),
 		angularDamp(0),
 		can_sleep(true),
+		omit_forces_integration(false),
 		force_integration_callback(NULL),
 		isTransformChanged(false),
 		previousActiveState(true),
@@ -326,6 +335,9 @@ 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() || previousActiveState != btBody->isActive()) && force_integration_callback && isTransformChanged) {
 
+		if (omit_forces_integration)
+			btBody->clearForces();
+
 		BulletPhysicsDirectBodyState *bodyDirect = BulletPhysicsDirectBodyState::get_singleton(this);
 
 		Variant variantBodyDirect = bodyDirect;
@@ -429,6 +441,10 @@ bool RigidBodyBullet::is_active() const {
 	return btBody->isActive();
 }
 
+void RigidBodyBullet::set_omit_forces_integration(bool p_omit) {
+	omit_forces_integration = p_omit;
+}
+
 void RigidBodyBullet::set_param(PhysicsServer::BodyParameter p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer::BODY_PARAM_BOUNCE:
diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h
index c4a9676bdd..b9511243c7 100644
--- a/modules/bullet/rigid_body_bullet.h
+++ b/modules/bullet/rigid_body_bullet.h
@@ -110,7 +110,9 @@ public:
 	virtual void set_transform(const Transform &p_transform);
 	virtual Transform get_transform() const;
 
+	virtual void add_central_force(const Vector3 &p_force);
 	virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos);
+	virtual void add_torque(const Vector3 &p_torque);
 	virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j);
 	virtual void apply_torque_impulse(const Vector3 &p_j);
 
@@ -196,6 +198,7 @@ private:
 	real_t linearDamp;
 	real_t angularDamp;
 	bool can_sleep;
+	bool omit_forces_integration;
 
 	Vector<CollisionData> collisions;
 	// these parameters are used to avoid vector resize
@@ -252,6 +255,9 @@ public:
 	void set_activation_state(bool p_active);
 	bool is_active() const;
 
+	void set_omit_forces_integration(bool p_omit);
+	_FORCE_INLINE_ bool get_omit_forces_integration() const { return omit_forces_integration; }
+
 	void set_param(PhysicsServer::BodyParameter p_param, real_t);
 	real_t get_param(PhysicsServer::BodyParameter p_param) const;
 
diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index c6b9695d96..76d9614465 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -125,18 +125,19 @@ btScaledBvhTriangleMeshShape *ShapeBullet::create_shape_concave(btBvhTriangleMes
 	}
 }
 
-btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size) {
+btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
 	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));
+	return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
 }
 
-btRayShape *ShapeBullet::create_shape_ray(real_t p_length) {
-	return bulletnew(btRayShape(p_length));
+btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) {
+	btRayShape *r(bulletnew(btRayShape(p_length)));
+	r->setSlipsOnSlope(p_slips_on_slope);
+	return r;
 }
 
 /* PLANE */
@@ -335,10 +336,10 @@ void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
 	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);
+
+		btTriangleMesh *shapeInterface = bulletnew(btTriangleMesh);
 		src_face_count /= 3;
 		PoolVector<Vector3>::Read r = p_faces.read();
 		const Vector3 *facesr = r.ptr();
@@ -385,19 +386,44 @@ void HeightMapShapeBullet::set_data(const Variant &p_data) {
 	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"));
 
+	real_t l_min_height = 0.0;
+	real_t l_max_height = 0.0;
+
+	// If specified, min and max height will be used as precomputed values
+	if (d.has("min_height"))
+		l_min_height = d["min_height"];
+	if (d.has("max_height"))
+		l_max_height = d["max_height"];
+
+	ERR_FAIL_COND(l_min_height > l_max_height);
+
 	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);
+	ERR_FAIL_COND(l_heights.size() != (l_width * l_depth));
+
+	// Compute min and max heights if not specified.
+	if (!d.has("min_height") && !d.has("max_height")) {
+
+		PoolVector<real_t>::Read r = heights.read();
+		int heights_size = heights.size();
+
+		for (int i = 0; i < heights_size; ++i) {
+			real_t h = r[i];
+
+			if (h < l_min_height)
+				l_min_height = h;
+			else if (h > l_max_height)
+				l_max_height = h;
+		}
+	}
+
+	setup(l_heights, l_width, l_depth, l_min_height, l_max_height);
 }
 
 Variant HeightMapShapeBullet::get_data() const {
@@ -408,8 +434,14 @@ 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) {
+void HeightMapShapeBullet::setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
+	// TODO cell size must be tweaked using localScaling, which is a shared property for all Bullet shapes
+
 	{ // Copy
+
+		// TODO If Godot supported 16-bit integer image format, we could share the same memory block for heightfields
+		// without having to copy anything, optimizing memory and loading performance (Bullet only reads and doesn't take ownership of the data).
+
 		const int heights_size = p_heights.size();
 		heights.resize(heights_size);
 		PoolVector<real_t>::Read p_heights_r = p_heights.read();
@@ -418,14 +450,16 @@ void HeightMapShapeBullet::setup(PoolVector<real_t> &p_heights, int p_width, int
 			heights_w[i] = p_heights_r[i];
 		}
 	}
+
 	width = p_width;
 	depth = p_depth;
-	cell_size = p_cell_size;
+	min_height = p_min_height;
+	max_height = p_max_height;
 	notifyShapeChanged();
 }
 
 btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
-	btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, cell_size));
+	btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, min_height, max_height));
 	cs->setLocalScaling(p_implicit_scale);
 	prepare(cs);
 	cs->setMargin(p_margin);
@@ -435,25 +469,33 @@ btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_impli
 /* Ray shape */
 RayShapeBullet::RayShapeBullet() :
 		ShapeBullet(),
-		length(1) {}
+		length(1),
+		slips_on_slope(false) {}
 
 void RayShapeBullet::set_data(const Variant &p_data) {
-	setup(p_data);
+
+	Dictionary d = p_data;
+	setup(d["length"], d["slips_on_slope"]);
 }
 
 Variant RayShapeBullet::get_data() const {
-	return length;
+
+	Dictionary d;
+	d["length"] = length;
+	d["slips_on_slope"] = slips_on_slope;
+	return d;
 }
 
 PhysicsServer::ShapeType RayShapeBullet::get_type() const {
 	return PhysicsServer::SHAPE_RAY;
 }
 
-void RayShapeBullet::setup(real_t p_length) {
+void RayShapeBullet::setup(real_t p_length, bool p_slips_on_slope) {
 	length = p_length;
+	slips_on_slope = p_slips_on_slope;
 	notifyShapeChanged();
 }
 
 btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
-	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_margin));
+	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_margin, slips_on_slope));
 }
diff --git a/modules/bullet/shape_bullet.h b/modules/bullet/shape_bullet.h
index e04a3c808a..abeea0f9ce 100644
--- a/modules/bullet/shape_bullet.h
+++ b/modules/bullet/shape_bullet.h
@@ -85,8 +85,8 @@ public:
 	/// 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);
+	static class btHeightfieldTerrainShape *create_shape_height_field(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
+	static class btRayShape *create_shape_ray(real_t p_length, bool p_slips_on_slope);
 };
 
 class PlaneShapeBullet : public ShapeBullet {
@@ -199,7 +199,8 @@ public:
 	PoolVector<real_t> heights;
 	int width;
 	int depth;
-	real_t cell_size;
+	real_t min_height;
+	real_t max_height;
 
 	HeightMapShapeBullet();
 
@@ -209,13 +210,14 @@ public:
 	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
 
 private:
-	void setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_cell_size);
+	void setup(PoolVector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
 };
 
 class RayShapeBullet : public ShapeBullet {
 
 public:
 	real_t length;
+	bool slips_on_slope;
 
 	RayShapeBullet();
 
@@ -225,6 +227,6 @@ public:
 	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
 
 private:
-	void setup(real_t p_length);
+	void setup(real_t p_length, bool p_slips_on_slope);
 };
 #endif
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 88d9c20eba..51a76ff8c5 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -628,7 +628,7 @@ void SpaceBullet::destroy_world() {
 
 void SpaceBullet::check_ghost_overlaps() {
 
-	/// Algorith support variables
+	/// Algorithm support variables
 	btConvexShape *other_body_shape;
 	btConvexShape *area_shape;
 	btGjkPairDetector::ClosestPointInput gjk_input;
@@ -660,7 +660,10 @@ void SpaceBullet::check_ghost_overlaps() {
 		// 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))
+			if (ghostOverlaps[i]->getUserIndex() == CollisionObjectBullet::TYPE_AREA) {
+				if (!static_cast<AreaBullet *>(ghostOverlaps[i]->getUserPointer())->is_monitorable())
+					continue;
+			} else if (ghostOverlaps[i]->getUserIndex() != CollisionObjectBullet::TYPE_RIGID_BODY)
 				continue;
 
 			otherObject = static_cast<RigidCollisionObjectBullet *>(ghostOverlaps[i]->getUserPointer());
@@ -790,7 +793,7 @@ void SpaceBullet::update_gravity() {
 /// I'm leaving this here just for future tests.
 /// Debug motion and normal vector drawing
 #define debug_test_motion 0
-#define PERFORM_INITIAL_UNSTACK 0
+
 #define RECOVERING_MOVEMENT_SCALE 0.4
 #define RECOVERING_MOVEMENT_CYCLES 4
 
@@ -804,8 +807,7 @@ 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, PhysicsServer::MotionResult *r_result) {
+bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result) {
 
 #if debug_test_motion
 	/// Yes I know this is not good, but I've used it as fast debugging hack.
@@ -839,25 +841,14 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 	}
 #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();
-	//    }
-	//}
-
 	btTransform body_safe_position;
 	G_TO_B(p_from, body_safe_position);
 	UNSCALE_BT_BASIS(body_safe_position);
 
-#if PERFORM_INITIAL_UNSTACK
 	btVector3 recover_initial_position(0, 0, 0);
 	{ /// Phase one - multi shapes depenetration using margin
 		for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
-			if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, recover_initial_position)) {
+			if (!recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, recover_initial_position)) {
 				break;
 			}
 		}
@@ -865,7 +856,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 		// Add recover movement in order to make it safe
 		body_safe_position.getOrigin() += recover_initial_position;
 	}
-#endif
 
 	btVector3 motion;
 	G_TO_B(p_motion, motion);
@@ -900,11 +890,11 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 			btTransform shape_world_to(shape_world_from);
 			shape_world_to.getOrigin() += motion;
 
-			GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, IGNORE_AREAS_TRUE);
+			GodotKinClosestConvexResultCallback btResult(shape_world_from.getOrigin(), shape_world_to.getOrigin(), p_body, p_infinite_inertia);
 			btResult.m_collisionFilterGroup = p_body->get_collision_layer();
 			btResult.m_collisionFilterMask = p_body->get_collision_mask();
 
-			dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, 0.002);
+			dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration);
 
 			if (btResult.hasHit()) {
 				/// Since for each sweep test I fix the motion of new shapes in base the recover result,
@@ -926,14 +916,11 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 		real_t l_penetration_distance = 1e20;
 
 		for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
-			l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, delta_recover_movement, &r_recover_result);
+			l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, delta_recover_movement, &r_recover_result);
 
 			if (r_result) {
-#if PERFORM_INITIAL_UNSTACK
 				B_TO_G(motion + delta_recover_movement + recover_initial_position, r_result->motion);
-#else
-				B_TO_G(motion + delta_recover_movement, r_result->motion);
-#endif
+
 				if (l_has_penetration) {
 					has_penetration = true;
 					if (l_penetration_distance <= r_recover_result.penetration_distance) {
@@ -955,15 +942,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f
 					r_result->collider_shape = r_recover_result.other_compound_shape_index;
 					r_result->collision_local_shape = r_recover_result.local_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(motion, sup_line2);
@@ -1022,7 +1000,7 @@ public:
 	}
 };
 
-bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
+bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) {
 
 	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
 
@@ -1053,7 +1031,7 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 
 		for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
 			btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
-			if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()))
+			if (!p_body->get_bt_collision_object()->checkCollideWith(otherObject) || !otherObject->checkCollideWith(p_body->get_bt_collision_object()) || (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()))
 				continue;
 
 			if (otherObject->getCollisionShape()->isCompound()) {
@@ -1132,7 +1110,7 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
 	btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, tA, -1, p_shapeId_A);
 	btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B);
 
-	btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS);
+	btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CONTACT_POINT_ALGORITHMS);
 	if (algorithm) {
 		GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB);
 		//discrete collision detection query
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index 2b97f0b274..a6c2786878 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -172,7 +172,7 @@ public:
 
 	void update_gravity();
 
-	bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, PhysicsServer::MotionResult *r_result);
+	bool test_body_motion(RigidBodyBullet *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer::MotionResult *r_result);
 
 private:
 	void create_empty_world(bool p_create_soft_world);
@@ -199,7 +199,7 @@ private:
 				local_shape_most_recovered(0) {}
 	};
 
-	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);
+	bool recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);
 	/// This is an API that recover a kinematic object from penetration
 	/// This allow only Convex Convex test and it always use GJK algorithm, With this API we don't benefit of Bullet special accelerated functions
 	bool RFP_convex_convex_test(const btConvexShape *p_shapeA, const btConvexShape *p_shapeB, btCollisionObject *p_objectB, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL);