Fixed bug in calculation of inertia for OffsetCenterOfMassShape (#402)

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -57,7 +57,12 @@ public:
 	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
 
 	// See Shape::GetMassProperties
-	virtual MassProperties			GetMassProperties() const override						{ return mInnerShape->GetMassProperties(); }
+	virtual MassProperties			GetMassProperties() const override
+	{
+		MassProperties mp = mInnerShape->GetMassProperties();
+		mp.Translate(mOffset);
+		return mp;
+	}
 
 	// See Shape::GetSubShapeTransformedShape
 	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;

UnitTests/Physics/OffsetCenterOfMassShapeTests.cpp

@@ -0,0 +1,76 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include "PhysicsTestContext.h"
+#include "Layers.h"
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+
+TEST_SUITE("OffsetCenterOfMassShapeTests")
+{
+	TEST_CASE("TestAngularImpulseCOMZero")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset 0
+		OffsetCenterOfMassShapeSettings com(Vec3::sZero(), &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		body.AddAngularImpulse(cImpulse);
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAngularImpulseCOMOffset")
+	{
+		PhysicsTestContext c;
+		c.ZeroGravity();
+
+		// Create box
+		const Vec3 cHalfExtent = Vec3(0.5f, 1.0f, 1.5f);
+		BoxShapeSettings box(cHalfExtent);
+		box.SetEmbedded();
+
+		// Create body with COM offset
+		const Vec3 cCOMOffset(5.0f, 0, 0);
+		OffsetCenterOfMassShapeSettings com(cCOMOffset, &box);
+		com.SetEmbedded();
+		Body &body = c.CreateBody(&com, RVec3::sZero(), Quat::sIdentity(), EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING, EActivation::DontActivate);
+
+		// Check mass and inertia calculated correctly
+		float mass = (8.0f * cHalfExtent.GetX() * cHalfExtent.GetY() * cHalfExtent.GetZ()) * box.mDensity;
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseMass(), 1.0f / mass);
+		float inertia_y = mass / 12.0f * (Square(2.0f * cHalfExtent.GetX()) + Square(2.0f * cHalfExtent.GetZ())) + mass * Square(cCOMOffset.GetX()); // See: https://en.wikipedia.org/wiki/List_of_moments_of_inertia & https://en.wikipedia.org/wiki/Parallel_axis_theorem
+		CHECK_APPROX_EQUAL(body.GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sIdentity())(1, 1), 1.0f / inertia_y);
+
+		// Add impulse
+		Vec3 cImpulse(0, 10000, 0);
+		body.AddAngularImpulse(cImpulse);
+
+		// Check resulting velocity change
+		// dv = I^-1 * L
+		float delta_v = (1.0f / inertia_y) * cImpulse.GetY();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+}

UnitTests/Physics/WheeledVehicleTests.cpp

@@ -321,7 +321,7 @@ TEST_SUITE("WheeledVehicleTests")
 				}
 			}
 			CHECK(vehicle_on_floor);
-			CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.02_r);
+			CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.03_r);
 
 			// Start driving
 			controller->SetDriverInput(1.0f, 0, 0, 0);
@@ -332,7 +332,7 @@ TEST_SUITE("WheeledVehicleTests")
 			if (t.mShouldMove)
 				CHECK(body->GetPosition().GetZ() > 0.5f);
 			else
-				CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.05_r);
+				CHECK_APPROX_EQUAL(body->GetPosition().GetZ(), 0, 0.06_r);
 		}
 	}
 }

UnitTests/UnitTests.cmake

@@ -43,6 +43,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Physics/ConvexVsTrianglesTest.cpp
 	${UNIT_TESTS_ROOT}/Physics/HeightFieldShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/MotionQualityLinearCastTests.cpp
+	${UNIT_TESTS_ROOT}/Physics/OffsetCenterOfMassShapeTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PathConstraintTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PhysicsDeterminismTests.cpp
 	${UNIT_TESTS_ROOT}/Physics/PhysicsStepListenerTests.cpp