Fix shape index in multiple physics queries with Bullet

Two main changes:

- Better handling of concave shapes to make sure the queries don't
return a triangle index instead of shape index.
Note: A concave shape within a compound shape will always return a shape
index of 0 because of Bullet limitations.

- Extra check for compound shapes in some queries to avoid undefined
behavior, because the shape index can have an uninitialized value with
convex shapes in some cases.

1 files changed, 41 insertions, 7 deletions

diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp
index 1f962772e7..edb2bcb4c7 100644
--- a/modules/bullet/godot_result_callbacks.cpp
+++ b/modules/bullet/godot_result_callbacks.cpp
@@ -107,7 +107,14 @@ btScalar GodotAllConvexResultCallback::addSingleResult(btCollisionWorld::LocalCo
 
 	PhysicsDirectSpaceState3D::ShapeResult &result = m_results[count];
 
-	result.shape = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is an odd name but contains the compound shape ID
+	// Triangle index is an odd name but contains the compound shape ID.
+	// A shape part of -1 indicates the index is a shape index and not a triangle index.
+	if (convexResult.m_localShapeInfo && convexResult.m_localShapeInfo->m_shapePart == -1) {
+		result.shape = convexResult.m_localShapeInfo->m_triangleIndex;
+	} else {
+		result.shape = 0;
+	}
+
 	result.rid = gObj->get_self();
 	result.collider_id = gObj->get_instance_id();
 	result.collider = result.collider_id.is_null() ? nullptr : ObjectDB::get_instance(result.collider_id);
@@ -171,11 +178,14 @@ bool GodotClosestConvexResultCallback::needsCollision(btBroadphaseProxy *proxy0)
 }
 
 btScalar GodotClosestConvexResultCallback::addSingleResult(btCollisionWorld::LocalConvexResult &convexResult, bool normalInWorldSpace) {
-	if (convexResult.m_localShapeInfo) {
-		m_shapeId = convexResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is an odd name but contains the compound shape ID
+	// Triangle index is an odd name but contains the compound shape ID.
+	// A shape part of -1 indicates the index is a shape index and not a triangle index.
+	if (convexResult.m_localShapeInfo && convexResult.m_localShapeInfo->m_shapePart == -1) {
+		m_shapeId = convexResult.m_localShapeInfo->m_triangleIndex;
 	} else {
 		m_shapeId = 0;
 	}
+
 	return btCollisionWorld::ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 }
 
@@ -219,10 +229,22 @@ btScalar GodotAllContactResultCallback::addSingleResult(btManifoldPoint &cp, con
 		CollisionObjectBullet *colObj;
 		if (m_self_object == colObj0Wrap->getCollisionObject()) {
 			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
-			result.shape = cp.m_index1;
+			// Checking for compound shape because the index might be uninitialized otherwise.
+			// A partId of -1 indicates the index is a shape index and not a triangle index.
+			if (colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId1 == -1) {
+				result.shape = cp.m_index1;
+			} else {
+				result.shape = 0;
+			}
 		} else {
 			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
-			result.shape = cp.m_index0;
+			// Checking for compound shape because the index might be uninitialized otherwise.
+			// A partId of -1 indicates the index is a shape index and not a triangle index.
+			if (colObj0Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId0 == -1) {
+				result.shape = cp.m_index0;
+			} else {
+				result.shape = 0;
+			}
 		}
 
 		result.collider_id = colObj->get_instance_id();
@@ -311,14 +333,26 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
 		CollisionObjectBullet *colObj;
 		if (m_self_object == colObj0Wrap->getCollisionObject()) {
 			colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
-			m_result->shape = cp.m_index1;
+			// Checking for compound shape because the index might be uninitialized otherwise.
+			// A partId of -1 indicates the index is a shape index and not a triangle index.
+			if (colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId1 == -1) {
+				m_result->shape = cp.m_index1;
+			} else {
+				m_result->shape = 0;
+			}
 			B_TO_G(cp.getPositionWorldOnB(), m_result->point);
 			B_TO_G(cp.m_normalWorldOnB, m_result->normal);
 			m_rest_info_bt_point = cp.getPositionWorldOnB();
 			m_rest_info_collision_object = colObj1Wrap->getCollisionObject();
 		} else {
 			colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
-			m_result->shape = cp.m_index0;
+			// Checking for compound shape because the index might be uninitialized otherwise.
+			// A partId of -1 indicates the index is a shape index and not a triangle index.
+			if (colObj0Wrap->getCollisionObject()->getCollisionShape()->isCompound() && cp.m_partId0 == -1) {
+				m_result->shape = cp.m_index0;
+			} else {
+				m_result->shape = 0;
+			}
 			B_TO_G(cp.m_normalWorldOnB * -1, m_result->normal);
 			m_rest_info_bt_point = cp.getPositionWorldOnA();
 			m_rest_info_collision_object = colObj0Wrap->getCollisionObject();