Added option to insert a shape in a MutableCompoundShape at specified index (#1414)

Fixed bug where bounding box could be too large after removing the last shape

See #1391

Docs/ReleaseNotes.md

@@ -15,6 +15,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added an example of a body that's both a sensor and a rigid body in `ContactListenerTest`.
 * Added binary serialization to `SkeletalAnimation`.
 * Added support for RISC-V, LoongArch and PowerPC (Little Endian) CPUs.
+* Added the ability to add a sub shape at a specified index in a MutableCompoundShape rather than at the end.
 
 ### Bug fixes
 
@@ -23,6 +24,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Added overloads for placement new in the `JPH_OVERRIDE_NEW_DELETE` macro, this means it is no longer needed to do `:: new (address) JPH::class_name(constructor_arguments)` but you can do `new (address) JPH::class_name(constructor_arguments)`.
 * Fixed a GCC warning `-Wshadow=global`.
 * BodyInterface::AddForce applied a force per soft body vertex rather than to the whole body, this resulted in a soft body accelerating much more compared to a rigid body of the same mass.
+* Removing a sub shape from a MutableCompoundShape would not update the bounding box if the last shape was removed, which can result in a small performance loss.
 
 ## v5.2.0
 

Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -206,29 +206,37 @@ void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumbe
 	CalculateLocalBounds();
 }
 
-uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
+uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData, uint inIndex)
 {
 	SubShape sub_shape;
 	sub_shape.mShape = inShape;
 	sub_shape.mUserData = inUserData;
 	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
-	mSubShapes.push_back(sub_shape);
-	uint shape_idx = (uint)mSubShapes.size() - 1;
 
-	CalculateSubShapeBounds(shape_idx, 1);
-
-	return shape_idx;
+	if (inIndex >= mSubShapes.size())
+	{
+		uint shape_idx = uint(mSubShapes.size());
+		mSubShapes.push_back(sub_shape);
+		CalculateSubShapeBounds(shape_idx, 1);
+		return shape_idx;
+	}
+	else
+	{
+		mSubShapes.insert(mSubShapes.begin() + inIndex, sub_shape);
+		CalculateSubShapeBounds(inIndex, uint(mSubShapes.size()) - inIndex);
+		return inIndex;
+	}
 }
 
 void MutableCompoundShape::RemoveShape(uint inIndex)
 {
 	mSubShapes.erase(mSubShapes.begin() + inIndex);
 
+	// We always need to recalculate the bounds of the sub shapes as we test blocks
+	// of 4 sub shapes at a time and removed shapes get their bounds updated
+	// to repeat the bounds of the previous sub shape
 	uint num_bounds = (uint)mSubShapes.size() - inIndex;
-	if (num_bounds > 0)
-		CalculateSubShapeBounds(inIndex, num_bounds);
-	else
-		CalculateLocalBounds();
+	CalculateSubShapeBounds(inIndex, num_bounds);
 }
 
 void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation)

Jolt/Physics/Collision/Shape/MutableCompoundShape.h

@@ -27,6 +27,8 @@ public:
 /// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition.
 /// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
 /// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+///
+/// When you modify a MutableCompoundShape, beware that the SubShapeIDs of all other shapes can change. So be careful when storing SubShapeIDs.
 class JPH_EXPORT MutableCompoundShape final : public CompoundShape
 {
 public:
@@ -66,8 +68,13 @@ public:
 
 	/// Adding a new shape.
 	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inPosition The position of the new shape
+	/// @param inRotation The orientation of the new shape
+	/// @param inShape The shape to add
+	/// @param inUserData User data that will be stored with the shape and can be retrieved using GetCompoundUserData
+	/// @param inIndex Index where to insert the shape, UINT_MAX to add to the end
 	/// @return The index of the newly added shape
-	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0);
+	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0, uint inIndex = UINT_MAX);
 
 	/// Remove a shape by index.
 	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.

UnitTests/Physics/MutableCompoundShapeTests.cpp

@@ -0,0 +1,173 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+
+TEST_SUITE("MutableCompoundShapeTests")
+{
+	TEST_CASE("TestMutableCompoundShapeAddRemove")
+	{
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> sphere1 = new SphereShape(1.0f);
+		settings.AddShape(Vec3::sZero(), Quat::sIdentity(), sphere1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+
+		auto check_shape_hit = [shape] (Vec3Arg inPosition) {
+			AllHitCollisionCollector<CollidePointCollector> collector;
+			shape->CollidePoint(inPosition - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+			SubShapeID remainder;
+			CHECK(collector.mHits.size() <= 1);
+			return !collector.mHits.empty()? shape->GetSubShape(shape->GetSubShapeIndexFromID(collector.mHits[0].mSubShapeID2, remainder)).mShape : nullptr;
+		};
+
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+
+		Ref<Shape> sphere2 = new SphereShape(2.0f);
+		shape->AddShape(Vec3(10, 0, 0), Quat::sIdentity(), sphere2, 0, 0); // Insert at the start
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+
+		Ref<Shape> sphere3 = new SphereShape(3.0f);
+		shape->AddShape(Vec3(20, 0, 0), Quat::sIdentity(), sphere3, 0, 2); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere1);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere1);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3(0, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere4 = new SphereShape(4.0f);
+		shape->AddShape(Vec3(0, 0, 0), Quat::sIdentity(), sphere4, 0); // Insert at the end
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetSubShape(2).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		Ref<Shape> sphere5 = new SphereShape(1.0f);
+		shape->AddShape(Vec3(15, 0, 0), Quat::sIdentity(), sphere5, 0, 1); // Insert in the middle
+		CHECK(shape->GetNumSubShapes() == 4);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetSubShape(3).mShape == sphere4);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-4, -4, -4), Vec3(23, 4, 4)));
+		CHECK(check_shape_hit(Vec3::sZero()) == sphere4);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(3);
+		CHECK(shape->GetNumSubShapes() == 3);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere5);
+		CHECK(shape->GetSubShape(2).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == sphere5);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 2);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetSubShape(1).mShape == sphere3);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -3, -3), Vec3(23, 3, 3)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == sphere3);
+
+		shape->RemoveShape(1);
+		CHECK(shape->GetNumSubShapes() == 1);
+		CHECK(shape->GetSubShape(0).mShape == sphere2);
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(8, -2, -2), Vec3(12, 2, 2)));
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == sphere2);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+
+		shape->RemoveShape(0);
+		CHECK(shape->GetNumSubShapes() == 0);
+		CHECK(!shape->GetLocalBounds().IsValid());
+		CHECK(check_shape_hit(Vec3::sZero()) == nullptr);
+		CHECK(check_shape_hit(Vec3(10, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(15, 0, 0)) == nullptr);
+		CHECK(check_shape_hit(Vec3(20, 0, 0)) == nullptr);
+	}
+
+	TEST_CASE("TestMutableCompoundShapeAdjustCenterOfMass")
+	{
+		// Start with a box at (-1 0 0)
+		MutableCompoundShapeSettings settings;
+		Ref<Shape> box_shape1 = new BoxShape(Vec3::sReplicate(1.0f));
+		box_shape1->SetUserData(1);
+		settings.AddShape(Vec3(-1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape1);
+		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3::sReplicate(-1.0f), Vec3::sReplicate(1.0f)));
+
+		// Check that we can hit the box
+		AllHitCollisionCollector<CollidePointCollector> collector;
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		CHECK(collector.mHits.empty());
+
+		// Now add another box at (1 0 0)
+		Ref<Shape> box_shape2 = new BoxShape(Vec3::sReplicate(1.0f));
+		box_shape2->SetUserData(2);
+		shape->AddShape(Vec3(1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape2);
+		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1.0f, -1.0f, -1.0f), Vec3(3.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+
+		// Adjust the center of mass
+		shape->AdjustCenterOfMass();
+		CHECK(shape->GetCenterOfMass() == Vec3::sZero());
+		CHECK(shape->GetLocalBounds() == AABox(Vec3(-2.0f, -1.0f, -1.0f), Vec3(2.0f, 1.0f, 1.0f)));
+
+		// Check that we can hit both boxes
+		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
+		collector.Reset();
+		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
+		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
+		collector.Reset();
+	}
+}

UnitTests/Physics/ShapeTests.cpp

@@ -896,51 +896,4 @@ TEST_SUITE("ShapeTests")
 			CHECK(user_data == expected_user_data);
 		}
 	}
-
-	TEST_CASE("TestMutableCompoundShapeAdjustCenterOfMass")
-	{
-		// Start with a box at (-1 0 0)
-		MutableCompoundShapeSettings settings;
-		Ref<Shape> box_shape1 = new BoxShape(Vec3::sReplicate(1.0f));
-		box_shape1->SetUserData(1);
-		settings.AddShape(Vec3(-1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape1);
-		Ref<MutableCompoundShape> shape = StaticCast<MutableCompoundShape>(settings.Create().Get());
-		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
-		CHECK(shape->GetLocalBounds() == AABox(Vec3::sReplicate(-1.0f), Vec3::sReplicate(1.0f)));
-
-		// Check that we can hit the box
-		AllHitCollisionCollector<CollidePointCollector> collector;
-		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
-		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
-		collector.Reset();
-		CHECK(collector.mHits.empty());
-
-		// Now add another box at (1 0 0)
-		Ref<Shape> box_shape2 = new BoxShape(Vec3::sReplicate(1.0f));
-		box_shape2->SetUserData(2);
-		shape->AddShape(Vec3(1.0f, 0.0f, 0.0f), Quat::sIdentity(), box_shape2);
-		CHECK(shape->GetCenterOfMass() == Vec3(-1.0f, 0.0f, 0.0f));
-		CHECK(shape->GetLocalBounds() == AABox(Vec3(-1.0f, -1.0f, -1.0f), Vec3(3.0f, 1.0f, 1.0f)));
-
-		// Check that we can hit both boxes
-		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
-		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
-		collector.Reset();
-		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
-		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
-		collector.Reset();
-
-		// Adjust the center of mass
-		shape->AdjustCenterOfMass();
-		CHECK(shape->GetCenterOfMass() == Vec3::sZero());
-		CHECK(shape->GetLocalBounds() == AABox(Vec3(-2.0f, -1.0f, -1.0f), Vec3(2.0f, 1.0f, 1.0f)));
-
-		// Check that we can hit both boxes
-		shape->CollidePoint(Vec3(-0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
-		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 1));
-		collector.Reset();
-		shape->CollidePoint(Vec3(0.5f, 0.0f, 0.0f) - shape->GetCenterOfMass(), SubShapeIDCreator(), collector);
-		CHECK((collector.mHits.size() == 1 && shape->GetSubShapeUserData(collector.mHits[0].mSubShapeID2) == 2));
-		collector.Reset();
-	}
 }

UnitTests/UnitTests.cmake

@@ -55,6 +55,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/HingeConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/MutableCompoundShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ObjectLayerPairFilterTableTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/ObjectLayerPairFilterMaskTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp