1 files changed, 13 insertions, 42 deletions

diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp
index 409d12f080..cbf30c8a2e 100644
--- a/modules/bullet/godot_result_callbacks.cpp
+++ b/modules/bullet/godot_result_callbacks.cpp
@@ -77,7 +77,7 @@ btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalCo
 
 	PhysicsDirectSpaceState::ShapeResult &result = m_results[count];
 
-	result.shape = convexResult.m_localShapeInfo->m_shapePart;
+	result.shape = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is a odd name but contains the compound shape ID
 	result.rid = gObj->get_self();
 	result.collider_id = gObj->get_instance_id();
 	result.collider = 0 == result.collider_id ? NULL : ObjectDB::get_instance(result.collider_id);
@@ -122,7 +122,7 @@ bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0)
 
 btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
 	btScalar res = btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
-	m_shapePart = convexResult.m_localShapeInfo->m_shapePart;
+	m_shapeId = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is a odd name but contains the compound shape ID
 	return res;
 }
 
@@ -243,49 +243,20 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
 	return cp.getDistance();
 }
 
-bool GodotRecoverAndClosestContactResultCallback::needsCollision(btBroadphaseProxy *proxy0) const {
-	const bool needs = GodotFilterCallback::test_collision_filters(m_collisionFilterGroup, m_collisionFilterMask, proxy0->m_collisionFilterGroup, proxy0->m_collisionFilterMask);
-	if (needs) {
-		btCollisionObject *btObj = static_cast<btCollisionObject *>(proxy0->m_clientObject);
-		CollisionObjectBullet *gObj = static_cast<CollisionObjectBullet *>(btObj->getUserPointer());
-		if (gObj == m_self_object) {
-			return false;
-		} else {
-			if (m_ignore_areas && gObj->getType() == CollisionObjectBullet::TYPE_AREA) {
-				return false;
-			} else if (m_self_object->has_collision_exception(gObj)) {
-				return false;
-			}
-		}
-		return true;
-	} else {
-		return false;
-	}
-}
+void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) {
 
-btScalar GodotRecoverAndClosestContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
-
-	if (cp.getDistance() < -MAX_PENETRATION_DEPTH) {
-		if (m_most_penetrated_distance > cp.getDistance()) {
-			m_most_penetrated_distance = cp.getDistance();
-
-			// take other object
-			btScalar sign(1);
-			if (m_self_object == colObj0Wrap->getCollisionObject()->getUserPointer()) {
-				m_pointCollisionObject = colObj1Wrap->getCollisionObject();
-				m_other_compound_shape_index = cp.m_index1;
-			} else {
-				m_pointCollisionObject = colObj0Wrap->getCollisionObject();
-				sign = -1;
-				m_other_compound_shape_index = cp.m_index0;
-			}
+	if (depth < 0) {
+		// Has penetration
+		if (m_most_penetrated_distance > depth) {
 
-			m_pointNormalWorld = cp.m_normalWorldOnB * sign;
-			m_pointWorld = cp.getPositionWorldOnB();
-			m_penetration_distance = cp.getDistance();
+			bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
 
-			m_recover_penetration -= cp.m_normalWorldOnB * sign * (cp.getDistance() + MAX_PENETRATION_DEPTH);
+			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;
 		}
 	}
-	return 1;
 }