Using more optimized MultiplyWorldSpaceInverseInertiaByVector in Add(Angular)Impulse (#404)

Jolt/Physics/Body/Body.inl

@@ -140,14 +140,14 @@ void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
 
 	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
 
-	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * Vec3(inPosition - mPosition).Cross(inImpulse));
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse)));
 }
 
 void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
 {
 	JPH_ASSERT(IsDynamic());
 
-	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * inAngularImpulse);
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse));
 }
 
 void Body::GetSleepTestPoints(RVec3 *outPoints) const

Samples/Tests/Shapes/OffsetCenterOfMassShapeTest.cpp

@@ -38,4 +38,19 @@ void OffsetCenterOfMassShapeTest::Initialize()
 	Body &body_right = *mBodyInterface->CreateBody(BodyCreationSettings(right, RVec3(5, 5, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING));
 	body_right.SetFriction(1.0f);
 	mBodyInterface->AddBody(body_right.GetID(), EActivation::Activate);
+
+	// Create body and apply a large angular impulse so see that it spins around the COM
+	BodyCreationSettings bcs(new OffsetCenterOfMassShapeSettings(Vec3(-3, 0, 0), new SphereShapeSettings(1.0f)), RVec3(-5, 5, 10), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mGravityFactor = 0.0f;
+	bcs.mLinearDamping = 0.0f;
+	bcs.mAngularDamping = 0.0f;
+	Body *body_rotating1 = mBodyInterface->CreateBody(bcs);
+    mBodyInterface->AddBody(body_rotating1->GetID(), EActivation::Activate);
+	body_rotating1->AddAngularImpulse(Vec3(0, 1.0e6f, 0));
+
+	// Create the same body but this time apply a torque
+	bcs.mPosition = RVec3(5, 5, 10);
+	Body *body_rotating2 = mBodyInterface->CreateBody(bcs);
+    mBodyInterface->AddBody(body_rotating2->GetID(), EActivation::Activate);
+	body_rotating2->AddTorque(Vec3(0, 1.0e6f * 60.0f, 0)); // Assuming physics sim is at 60Hz here, otherwise the bodies won't rotate with the same speed
 }

UnitTests/Physics/OffsetCenterOfMassShapeTests.cpp

@@ -9,7 +9,7 @@
 
 TEST_SUITE("OffsetCenterOfMassShapeTests")
 {
-	TEST_CASE("TestAngularImpulseCOMZero")
+	TEST_CASE("TestAddAngularImpulseCOMZero")
 	{
 		PhysicsTestContext c;
 		c.ZeroGravity();
@@ -32,7 +32,9 @@ TEST_SUITE("OffsetCenterOfMassShapeTests")
 
 		// Add impulse
 		Vec3 cImpulse(0, 10000, 0);
-		body.AddAngularImpulse(cImpulse);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
 
 		// Check resulting velocity change
 		// dv = I^-1 * L
@@ -41,7 +43,7 @@ TEST_SUITE("OffsetCenterOfMassShapeTests")
 		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
 	}
 
-	TEST_CASE("TestAngularImpulseCOMOffset")
+	TEST_CASE("TestAddAngularImpulseCOMOffset")
 	{
 		PhysicsTestContext c;
 		c.ZeroGravity();
@@ -65,7 +67,9 @@ TEST_SUITE("OffsetCenterOfMassShapeTests")
 
 		// Add impulse
 		Vec3 cImpulse(0, 10000, 0);
-		body.AddAngularImpulse(cImpulse);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddAngularImpulse(body.GetID(), cImpulse);
+		CHECK(body.IsActive());
 
 		// Check resulting velocity change
 		// dv = I^-1 * L
@@ -73,4 +77,77 @@ TEST_SUITE("OffsetCenterOfMassShapeTests")
 		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
 		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
 	}
+	
+	TEST_CASE("TestAddTorqueCOMZero")
+	{
+		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 torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
+
+	TEST_CASE("TestAddTorqueCOMOffset")
+	{
+		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 torque
+		Vec3 cTorque(0, 100000, 0);
+		CHECK(!body.IsActive());
+		c.GetBodyInterface().AddTorque(body.GetID(), cTorque);
+		CHECK(body.IsActive());
+		CHECK(body.GetAngularVelocity() == Vec3::sZero()); // Angular velocity change should come after the next time step
+		c.SimulateSingleStep();
+
+		// Check resulting velocity change
+		// dv = I^-1 * T * dt
+		float delta_v = (1.0f / inertia_y) * cTorque.GetY() * c.GetDeltaTime();
+		CHECK_APPROX_EQUAL(body.GetLinearVelocity(), Vec3::sZero());
+		CHECK_APPROX_EQUAL(body.GetAngularVelocity(), Vec3(0, delta_v, 0));
+	}
 }