Fix shape index in multiple physics queries with Bullet

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();

modules/bullet/godot_result_callbacks.h

@@ -70,8 +70,10 @@ struct GodotClosestRayResultCallback : public btCollisionWorld::ClosestRayResult
  virtual bool needsCollision(btBroadphaseProxy *proxy0) const;
 
  virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult &rayResult, bool normalInWorldSpace) {
- if (rayResult.m_localShapeInfo) {
- m_shapeId = rayResult.m_localShapeInfo->m_triangleIndex; // "m_triangleIndex" Is a 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 (rayResult.m_localShapeInfo && rayResult.m_localShapeInfo->m_shapePart == -1) {
+ m_shapeId = rayResult.m_localShapeInfo->m_triangleIndex;
  } else {
  m_shapeId = 0;
  }