Merge pull request #27415 from aqnuep/kinematicbody_fixes

KinematicBody performance and quality improvements

6 files changed, 225 insertions, 104 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 3a90bdc6ae..eb87901c24 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