Slightly simplified capsule inertia calculation (#837)

* Also added remark that the article contains an error (see #830)
* Added unit test that compares inertia of convex hull with capsule

Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -219,29 +219,27 @@ MassProperties CapsuleShape::GetMassProperties() const
 
 	// Calculate inertia and mass according to:
 	// https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856
-	float radius_sq = mRadius * mRadius;
+	// Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value
+	float radius_sq = Square(mRadius);
 	float height = 2.0f * mHalfHeightOfCylinder;
 	float cylinder_mass = JPH_PI * height * radius_sq * density;
 	float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density;
 
 	// From cylinder
+	float height_sq = Square(height);
 	float inertia_y = radius_sq * cylinder_mass * 0.5f;
-	float inertia_x = inertia_y * 0.5f + cylinder_mass * height * height / 12.0f;
-	float inertia_z = inertia_x;
+	float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f;
 
 	// From hemispheres
-	float temp0 = hemisphere_mass * 2.0f * radius_sq / 5.0f;
-	inertia_y += temp0  *  2.0f;
-	float temp1 = mHalfHeightOfCylinder;
-	float temp2 = temp0 + hemisphere_mass * (temp1 * temp1 + (3.0f / 8.0f) * height * mRadius);
-	inertia_x += temp2 * 2.0f;
-	inertia_z += temp2 * 2.0f;
+	float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f;
+	inertia_y += temp;
+	inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius);
 
 	// Mass is cylinder + hemispheres
 	p.mMass = cylinder_mass + hemisphere_mass * 2.0f;
 
 	// Set inertia
-	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
+	p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz));
 
 	return p;
 }

UnitTests/Physics/ShapeTests.cpp

@@ -64,6 +64,59 @@ TEST_SUITE("ShapeTests")
 		CHECK_APPROX_EQUAL(shape->GetInnerRadius(), 2.5f);
 	}
 
+	// Test inertia calculations for a capsule vs that of a convex hull of a capsule
+	TEST_CASE("TestCapsuleVsConvexHullInertia")
+	{
+		const float half_height = 5.0f;
+		const float radius = 3.0f;
+
+		// Create a capsule
+		CapsuleShape capsule(half_height, radius);
+		capsule.SetDensity(7.0f);
+		capsule.SetEmbedded();
+		MassProperties mp_capsule = capsule.GetMassProperties();
+
+		// Verify mass
+		float mass_cylinder = 2.0f * half_height * JPH_PI * Square(radius) * capsule.GetDensity();
+		float mass_sphere = 4.0f / 3.0f * JPH_PI * Cubed(radius) * capsule.GetDensity();
+		CHECK_APPROX_EQUAL(mp_capsule.mMass, mass_cylinder + mass_sphere);
+
+		// Extract support points
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = capsule.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+		Array<Vec3> capsule_points;
+		capsule_points.reserve(Vec3::sUnitSphere.size());
+		for (const Vec3 &v : Vec3::sUnitSphere)
+			capsule_points.push_back(support->GetSupport(v));
+
+		// Create a convex hull using the support points
+		ConvexHullShapeSettings capsule_hull(capsule_points);
+		capsule_hull.SetDensity(capsule.GetDensity());
+		RefConst<Shape> capsule_hull_shape = capsule_hull.Create().Get();
+		MassProperties mp_capsule_hull = capsule_hull_shape->GetMassProperties();
+
+		// Check that the mass and inertia of the convex hull match that of the capsule (within certain tolerance since the convex hull is an approximation)
+		float mass_error = (mp_capsule_hull.mMass - mp_capsule.mMass) / mp_capsule.mMass;
+		CHECK(mass_error > -0.05f);
+		CHECK(mass_error < 0.0f); // Mass is smaller since the convex hull is smaller
+		for (int i = 0; i < 3; ++i)
+			for (int j = 0; j < 3; ++j)
+			{
+				if (i == j)
+				{
+					float inertia_error = (mp_capsule_hull.mInertia(i, j) - mp_capsule.mInertia(i, j)) / mp_capsule.mInertia(i, j);
+					CHECK(inertia_error > -0.05f);
+					CHECK(inertia_error < 0.0f); // Inertia is smaller since the convex hull is smaller
+				}
+				else
+				{
+					CHECK(mp_capsule.mInertia(i, j) == 0.0f);
+					float scaled_inertia = mp_capsule_hull.mInertia(i, j) / mp_capsule_hull.mMass;
+					CHECK_APPROX_EQUAL(scaled_inertia, 0.0f, 1.0e-3f);
+				}
+			}
+	}
+
 	// Test IsValidScale function
 	TEST_CASE("TestIsValidScale")
 	{