KinematicBody performance and quality improvements

With this change finally one can use compound collisions (like those created
by Gridmaps) without serious performance issues. The previous KinematicBody
code for Bullet was practically doing a whole bunch of unnecessary
calculations. Gridmaps with fairly large octant sizes (in my case 32) can get
up to 10000x speedup with this change (literally!). I expect the FPS demo to
get a fair speedup as well.

List of fixes and improvements:

- Fixed a general bug in move_and_slide that affects both GodotPhysics and
  Bullet, where ray shapes would be ignored unless the stop_on_slope parameter
  is disabled. Not sure where that came from, but looking at the 2D physics
  code it was obvious there's a difference.
- Enabled the dynamic AABB tree that Bullet uses to allow broadphase collision
  tests against individual shapes of compound shapes. This is crucial to get
  good performance with Gridmaps and in general improves the performance
  whenever a KinematicBody collides with compound collision shapes.
- Added code to the broadphase collision detection code used by the Bullet
  module for KinematicBodies to also do broadphase on the sub-shapes of
  compound collision shapes. This is possible thanks to the dynamic AABB
  tree that was previously disabled and it's the change that provides the
  biggest performance boost.
- Now broadphase test is only done once per KinematicBody in Bullet instead of
  once per each of its shapes which was completely unnecessary.
- Fixed the way how the ray separation results are populated in Bullet which
  was completely broken previously, overwriting previous results and similar
  non-sense.
- Fixed ray shapes for good now. Previously the margin set in the editor was
  not respected at all, and the KinematicBody code for ray separation was
  complete bogus, thus all previous attempts to fix it were mislead.
- Fixed an obvious bug also in GodotPhysics where an out-of-bounds index was
  used in the ray result array.

There are a whole set of other problems with the KinematicBody code of Bullet
which cost performance and may cause unexpected behavior, but those are not
addressed in this change (need to keep it "simple").

Not sure whether this fixes any outstanding Github issues but I wouldn't be
surprised.

9 files changed, 235 insertions, 120 deletions

diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp
index b902d08eca..b60d6ba693 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::setMargin(btScalar margin) {
+	btConvexInternalShape::setMargin(margin);
+	reload_cache();
+}
+
 void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) {
 
 	slipsOnSlope = p_slipsOnSlope;
@@ -77,10 +82,9 @@ void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVecto
 }
 
 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_cacheScaledLength + m_collisionMargin));
-	btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax);
+	btVector3 localAabbMax(m_shapeAxis * m_cacheScaledLength);
+	btTransformAabb(localAabbMin, localAabbMax, m_collisionMargin, t, aabbMin, aabbMax);
 }
 
 void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
@@ -100,5 +104,5 @@ void btRayShape::reload_cache() {
 	m_cacheScaledLength = m_length * m_localScaling[2];
 
 	m_cacheSupportPoint.setIdentity();
-	m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin));
+	m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength);
 }
diff --git a/modules/bullet/btRayShape.h b/modules/bullet/btRayShape.h
index 7fedb74083..7f3229b3e8 100644
--- a/modules/bullet/btRayShape.h
+++ b/modules/bullet/btRayShape.h
@@ -60,6 +60,8 @@ public:
 	void setLength(btScalar p_length);
 	btScalar getLength() const { return m_length; }
 
+	virtual void setMargin(btScalar margin);
+
 	void setSlipsOnSlope(bool p_slipOnSlope);
 	bool getSlipsOnSlope() const { return slipsOnSlope; }
 
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index ef5f21fc21..6dab555fef 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -43,7 +43,9 @@
 	@author AndreaCatania
 */
 
-#define enableDynamicAabbTree false
+// We enable dynamic AABB tree so that we can actually perform a broadphase on bodies with compound collision shapes.
+// This is crucial for the performance of kinematic bodies and for bodies with transforming shapes.
+#define enableDynamicAabbTree true
 
 CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
 
@@ -284,7 +286,6 @@ void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transfor
 	ERR_FAIL_INDEX(p_index, get_shape_count());
 
 	shapes.write[p_index].set_transform(p_transform);
-	// Note, enableDynamicAabbTree is false because on transform change compound is destroyed
 	reload_shapes();
 }
 
diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp
index 3e06239453..2ba75b9a98 100644
--- a/modules/bullet/godot_ray_world_algorithm.cpp
+++ b/modules/bullet/godot_ray_world_algorithm.cpp
@@ -39,6 +39,9 @@
 	@author AndreaCatania
 */
 
+// Epsilon to account for floating point inaccuracies
+#define RAY_PENETRATION_DEPTH_EPSILON 0.01
+
 GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) :
 		m_world(world) {}
 
@@ -100,8 +103,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo
 
 		btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1));
 
-		if (depth >= -ray_shape->getMargin() * 0.5)
-			depth = 0;
+		if (depth > -RAY_PENETRATION_DEPTH_EPSILON)
+			depth = 0.0;
 
 		if (ray_shape->getSlipsOnSlope())
 			resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth);
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 8fb8eba057..6bfd98873e 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -1043,23 +1043,16 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
 	btVector3 recover_motion(0, 0, 0);
 
 	int rays_found = 0;
+	int rays_found_this_round = 0;
 
 	for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) {
-		int last_ray_index = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max, recover_motion, r_results);
+		PhysicsServer::SeparationResult *next_results = &r_results[rays_found];
+		rays_found_this_round = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max - rays_found, recover_motion, next_results);
 
-		rays_found = MAX(last_ray_index, rays_found);
-		if (!rays_found) {
-			break;
-		} else {
+		rays_found += rays_found_this_round;
+		if (rays_found_this_round == 0) {
 			body_transform.getOrigin() += recover_motion;
-		}
-	}
-
-	//optimize results (remove non colliding)
-	for (int i = 0; i < rays_found; i++) {
-		if (r_results[i].collision_depth >= 0) {
-			rays_found--;
-			SWAP(r_results[i], r_results[rays_found]);
+			break;
 		}
 	}
 
@@ -1069,18 +1062,47 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p
 
 struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback {
 private:
+	btDbvtVolume bounds;
+
 	const btCollisionObject *self_collision_object;
 	uint32_t collision_layer;
 	uint32_t collision_mask;
 
+	struct CompoundLeafCallback : btDbvt::ICollide {
+	private:
+		RecoverPenetrationBroadPhaseCallback *parent_callback;
+		btCollisionObject *collision_object;
+
+	public:
+		CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) :
+				parent_callback(p_parent_callback),
+				collision_object(p_collision_object) {
+		}
+
+		void Process(const btDbvtNode *leaf) {
+			BroadphaseResult result;
+			result.collision_object = collision_object;
+			result.compound_child_index = leaf->dataAsInt;
+			parent_callback->results.push_back(result);
+		}
+	};
+
 public:
-	Vector<btCollisionObject *> result_collision_objects;
+	struct BroadphaseResult {
+		btCollisionObject *collision_object;
+		int compound_child_index;
+	};
+
+	Vector<BroadphaseResult> results;
 
 public:
-	RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask) :
+	RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask, btVector3 p_aabb_min, btVector3 p_aabb_max) :
 			self_collision_object(p_self_collision_object),
 			collision_layer(p_collision_layer),
-			collision_mask(p_collision_mask) {}
+			collision_mask(p_collision_mask) {
+
+		bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max);
+	}
 
 	virtual ~RecoverPenetrationBroadPhaseCallback() {}
 
@@ -1089,35 +1111,98 @@ public:
 		btCollisionObject *co = static_cast<btCollisionObject *>(proxy->m_clientObject);
 		if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) {
 			if (self_collision_object != proxy->m_clientObject && GodotFilterCallback::test_collision_filters(collision_layer, collision_mask, proxy->m_collisionFilterGroup, proxy->m_collisionFilterMask)) {
-				result_collision_objects.push_back(co);
+				if (co->getCollisionShape()->isCompound()) {
+					const btCompoundShape *cs = static_cast<btCompoundShape *>(co->getCollisionShape());
+
+					if (cs->getNumChildShapes() > 1) {
+						const btDbvt *tree = cs->getDynamicAabbTree();
+						ERR_FAIL_COND_V(tree == NULL, true);
+
+						// Transform bounds into compound shape local space
+						const btTransform other_in_compound_space = co->getWorldTransform().inverse();
+						const btMatrix3x3 abs_b = other_in_compound_space.getBasis().absolute();
+						const btVector3 local_center = other_in_compound_space(bounds.Center());
+						const btVector3 local_extent = bounds.Extents().dot3(abs_b[0], abs_b[1], abs_b[2]);
+						const btVector3 local_aabb_min = local_center - local_extent;
+						const btVector3 local_aabb_max = local_center + local_extent;
+						const btDbvtVolume local_bounds = btDbvtVolume::FromMM(local_aabb_min, local_aabb_max);
+
+						// Test collision against compound child shapes using its AABB tree
+						CompoundLeafCallback compound_leaf_callback(this, co);
+						tree->collideTV(tree->m_root, local_bounds, compound_leaf_callback);
+					} else {
+						// If there's only a single child shape then there's no need to search any more, we know which child overlaps
+						BroadphaseResult result;
+						result.collision_object = co;
+						result.compound_child_index = 0;
+						results.push_back(result);
+					}
+				} else {
+					BroadphaseResult result;
+					result.collision_object = co;
+					result.compound_child_index = -1;
+					results.push_back(result);
+				}
 				return true;
 			}
 		}
 		return false;
 	}
-
-	void reset() {
-		result_collision_objects.clear();
-	}
 };
 
 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());
+	// Calculate the cummulative AABB of all shapes of the kinematic body
+	btVector3 aabb_min, aabb_max;
+	bool shapes_found = false;
+
+	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
+
+		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
+		if (!kin_shape.is_active()) {
+			continue;
+		}
+
+		if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {
+			// Skip rayshape in order to implement custom separation process
+			continue;
+		}
+
+		btTransform shape_transform = p_body_position * kin_shape.transform;
+		shape_transform.getOrigin() += r_delta_recover_movement;
+
+		btVector3 shape_aabb_min, shape_aabb_max;
+		kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
+
+		if (!shapes_found) {
+			aabb_min = shape_aabb_min;
+			aabb_max = shape_aabb_max;
+			shapes_found = true;
+		} else {
+			aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
+			aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
+			aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
+
+			aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
+			aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
+			aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
+		}
+	}
 
-	btTransform body_shape_position;
-	btTransform body_shape_position_recovered;
+	// If there are no shapes then there is no penetration either
+	if (!shapes_found) {
+		return false;
+	}
 
-	// Broad phase support
-	btVector3 minAabb, maxAabb;
+	// Perform broadphase test
+	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max);
+	dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
 
 	bool penetration = false;
 
-	// For each shape
+	// Perform narrowphase per shape
 	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
 
-		recover_broad_result.reset();
-
 		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
 		if (!kin_shape.is_active()) {
 			continue;
@@ -1128,15 +1213,11 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 			continue;
 		}
 
-		body_shape_position = p_body_position * kin_shape.transform;
-		body_shape_position_recovered = body_shape_position;
-		body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
+		btTransform shape_transform = p_body_position * kin_shape.transform;
+		shape_transform.getOrigin() += r_delta_recover_movement;
 
-		kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
-		dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
-
-		for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
-			btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
+		for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
+			btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
 			if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
 				otherObject->activate(); // Force activation of hitten rigid, soft body
 				continue;
@@ -1144,30 +1225,28 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran
 				continue;
 
 			if (otherObject->getCollisionShape()->isCompound()) {
+				const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
+				int shape_idx = recover_broad_result.results[i].compound_child_index;
+				ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
 
-				// Each convex shape
-				btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
-				for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
-
-					if (cs->getChildShape(x)->isConvex()) {
-						if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(x)), otherObject, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
+				if (cs->getChildShape(shape_idx)->isConvex()) {
+					if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(cs->getChildShape(shape_idx)), otherObject, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
 
-							penetration = true;
-						}
-					} else {
-						if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
+						penetration = true;
+					}
+				} else {
+					if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
 
-							penetration = true;
-						}
+						penetration = true;
 					}
 				}
 			} else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape
-				if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
+				if (RFP_convex_convex_test(kin_shape.shape, static_cast<const btConvexShape *>(otherObject->getCollisionShape()), otherObject, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
 
 					penetration = true;
 				}
 			} else {
-				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
+				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) {
 
 					penetration = true;
 				}
@@ -1183,7 +1262,6 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt
 	// Initialize GJK input
 	btGjkPairDetector::ClosestPointInput gjk_input;
 	gjk_input.m_transformA = p_transformA;
-	gjk_input.m_transformA.getOrigin() += r_delta_recover_movement;
 	gjk_input.m_transformB = p_transformB;
 
 	// Perform GJK test
@@ -1214,7 +1292,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
 	/// Contact test
 
 	btTransform tA(p_transformA);
-	tA.getOrigin() += r_delta_recover_movement;
 
 	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);
@@ -1246,39 +1323,81 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC
 	return false;
 }
 
-void SpaceBullet::convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
+int SpaceBullet::add_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const {
+
+	// optimize results (ignore non-colliding)
+	if (p_recover_result.penetration_distance < 0.0) {
+		const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
+		CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
 
-	const btRigidBody *btRigid = static_cast<const btRigidBody *>(p_other_object);
-	CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(p_other_object->getUserPointer());
+		r_result->collision_depth = p_recover_result.penetration_distance;
+		B_TO_G(p_recover_result.pointWorld, r_result->collision_point);
+		B_TO_G(p_recover_result.normal, r_result->collision_normal);
+		B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity);
+		r_result->collision_local_shape = p_shape_id;
+		r_result->collider_id = collisionObject->get_instance_id();
+		r_result->collider = collisionObject->get_self();
+		r_result->collider_shape = p_recover_result.other_compound_shape_index;
 
-	r_result->collision_depth = p_recover_result.penetration_distance;
-	B_TO_G(p_recover_result.pointWorld, r_result->collision_point);
-	B_TO_G(p_recover_result.normal, r_result->collision_normal);
-	B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity);
-	r_result->collision_local_shape = p_shape_id;
-	r_result->collider_id = collisionObject->get_instance_id();
-	r_result->collider = collisionObject->get_self();
-	r_result->collider_shape = p_recover_result.other_compound_shape_index;
+		return 1;
+	} else {
+		return 0;
+	}
 }
 
 int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results) {
 
-	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask());
+	// Calculate the cummulative AABB of all shapes of the kinematic body
+	btVector3 aabb_min, aabb_max;
+	bool shapes_found = false;
+
+	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
+
+		const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]);
+		if (!kin_shape.is_active()) {
+			continue;
+		}
+
+		if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) {
+			continue;
+		}
+
+		btTransform shape_transform = p_body_position * kin_shape.transform;
+		shape_transform.getOrigin() += r_delta_recover_movement;
 
-	btTransform body_shape_position;
-	btTransform body_shape_position_recovered;
+		btVector3 shape_aabb_min, shape_aabb_max;
+		kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max);
 
-	// Broad phase support
-	btVector3 minAabb, maxAabb;
+		if (!shapes_found) {
+			aabb_min = shape_aabb_min;
+			aabb_max = shape_aabb_max;
+			shapes_found = true;
+		} else {
+			aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x());
+			aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y());
+			aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z());
 
-	int ray_index = 0;
+			aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x());
+			aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y());
+			aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z());
+		}
+	}
 
-	// For each shape
-	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
+	// If there are no shapes then there is no penetration either
+	if (!shapes_found) {
+		return 0;
+	}
+
+	// Perform broadphase test
+	RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max);
+	dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result);
 
-		recover_broad_result.reset();
+	int ray_count = 0;
+
+	// Perform narrowphase per shape
+	for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) {
 
-		if (ray_index >= p_result_max) {
+		if (ray_count >= p_result_max) {
 			break;
 		}
 
@@ -1291,15 +1410,11 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
 			continue;
 		}
 
-		body_shape_position = p_body_position * kin_shape.transform;
-		body_shape_position_recovered = body_shape_position;
-		body_shape_position_recovered.getOrigin() += r_delta_recover_movement;
+		btTransform shape_transform = p_body_position * kin_shape.transform;
+		shape_transform.getOrigin() += r_delta_recover_movement;
 
-		kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb);
-		dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result);
-
-		for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) {
-			btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i];
+		for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) {
+			btCollisionObject *otherObject = recover_broad_result.results[i].collision_object;
 			if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) {
 				otherObject->activate(); // Force activation of hitten rigid, soft body
 				continue;
@@ -1307,29 +1422,25 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT
 				continue;
 
 			if (otherObject->getCollisionShape()->isCompound()) {
+				const btCompoundShape *cs = static_cast<const btCompoundShape *>(otherObject->getCollisionShape());
+				int shape_idx = recover_broad_result.results[i].compound_child_index;
+				ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false);
 
-				// Each convex shape
-				btCompoundShape *cs = static_cast<btCompoundShape *>(otherObject->getCollisionShape());
-				for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) {
-
-					RecoverResult recover_result;
-					if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
+				RecoverResult recover_result;
+				if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
 
-						convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject);
-					}
+					ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
 				}
 			} else {
 
 				RecoverResult recover_result;
-				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
+				if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) {
 
-					convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject);
+					ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject);
 				}
 			}
 		}
-
-		++ray_index;
 	}
 
-	return ray_index;
+	return ray_count;
 }
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index 7bf6a216b5..6b3d65edf6 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -212,7 +212,7 @@ private:
 	/// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm
 	bool RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, 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);
 
-	void convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const;
+	int add_separation_result(PhysicsServer::SeparationResult *r_results, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const;
 	int recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results);
 };
 #endif
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 05214ed669..90cc415942 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1181,19 +1181,16 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 	while (p_max_slides) {
 
 		Collision collision;
-
 		bool found_collision = false;
 
-		int test_type = 0;
-
-		do {
+		for (int i = 0; i < 2; ++i) {
 			bool collided;
-			if (test_type == 0) { //collide
+			if (i == 0) { //collide
 				collided = move_and_collide(motion, p_infinite_inertia, collision);
 				if (!collided) {
 					motion = Vector3(); //clear because no collision happened and motion completed
 				}
-			} else {
+			} else { //separate raycasts (if any)
 				collided = separate_raycast_shapes(p_infinite_inertia, collision);
 				if (collided) {
 					collision.remainder = motion; //keep
@@ -1222,7 +1219,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 						floor_velocity = collision.collider_vel;
 
 						if (p_stop_on_slope) {
-							if ((lv_n + p_floor_direction).length() < 0.01) {
+							if ((lv_n + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
 								Transform gt = get_global_transform();
 								gt.origin -= collision.travel;
 								set_global_transform(gt);
@@ -1243,21 +1240,18 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 					motion = motion.slide(p_floor_direction);
 					lv = lv.slide(p_floor_direction);
 				} else {
-
 					Vector3 n = collision.normal;
 					motion = motion.slide(n);
 					lv = lv.slide(n);
 				}
 
-				for (int i = 0; i < 3; i++) {
-					if (locked_axis & (1 << i)) {
-						lv[i] = 0;
+				for (int j = 0; j < 3; j++) {
+					if (locked_axis & (1 << j)) {
+						lv[j] = 0;
 					}
 				}
 			}
-
-			++test_type;
-		} while (!p_stop_on_slope && test_type < 2);
+		}
 
 		if (!found_collision || motion == Vector3())
 			break;
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 589af98062..aa6030d44e 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -317,7 +317,7 @@ protected:
 	static void _bind_methods();
 
 public:
-	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
+	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);
 
 	bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision);
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 4ab92715f4..9920391471 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -632,7 +632,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
 
 						int ray_index = -1; //reuse shape
 						for (int k = 0; k < rays_found; k++) {
-							if (r_results[ray_index].collision_local_shape == j) {
+							if (r_results[k].collision_local_shape == j) {
 								ray_index = k;
 							}
 						}