Bugfix: MutableCompoundShape::AdjustCenterOfMass did not update bounding box (#1169)

Fixes #1168

Docs/ReleaseNotes.md

@@ -64,6 +64,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Forgot to free a temporary allocation on an early out in HeightFieldShape::SetMaterials.
 * Fix SSE not being enabled on x86 32-bits.
 * Fixed a bug in the enhanced internal edge removal that could cause rigid bodies and characters to be affected by internal edges.
+* Fixed a bug in MutableCompoundShape::AdjustCenterOfMass which would fail to update the bounding box of the shape.
 
 ## v5.0.0
 

Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp

@@ -87,6 +87,21 @@ void MutableCompoundShape::AdjustCenterOfMass()
 	for (CompoundShape::SubShape &sub_shape : mSubShapes)
 		sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass);
 
+	// Update bounding boxes
+	for (Bounds &bounds : mSubShapeBounds)
+	{
+		Vec4 xxxx = center_of_mass.SplatX();
+		Vec4 yyyy = center_of_mass.SplatY();
+		Vec4 zzzz = center_of_mass.SplatZ();
+		bounds.mMinX -= xxxx;
+		bounds.mMinY -= yyyy;
+		bounds.mMinZ -= zzzz;
+		bounds.mMaxX -= xxxx;
+		bounds.mMaxY -= yyyy;
+		bounds.mMaxZ -= zzzz;
+	}
+	mLocalBounds.Translate(-center_of_mass);
+
 	// And adjust the center of mass for this shape in the opposite direction
 	mCenterOfMass += center_of_mass;
 }
@@ -162,7 +177,7 @@ void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumbe
 			{
 				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
 
-				// Tranform the shape's bounds into our local space
+				// Transform the shape's bounds into our local space
 				Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM());
 
 				// Get the bounding box
@@ -174,7 +189,7 @@ void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumbe
 			bounds_max.SetColumn3(col, sub_shape_bounds.mMax);
 		}
 
-		// Transpose to go to strucucture of arrays format
+		// Transpose to go to structure of arrays format
 		Mat44 bounds_min_t = bounds_min.Transposed();
 		Mat44 bounds_max_t = bounds_max.Transposed();
 

UnitTests/Physics/ShapeTests.cpp

@@ -748,4 +748,51 @@ TEST_SUITE("ShapeTests")
 			CHECK(mesh_shape->GetSurfaceNormal(hit.mSubShapeID2, ray.GetPointOnRay(hit.mFraction)) == Vec3::sAxisY());
 		}
 	}
+
+	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();
+	}
 }