Added ScaleToMass function (#1326)

This function lets you easily change the mass of a body

Jolt/Physics/Body/MotionProperties.h

@@ -97,7 +97,7 @@ public:
 
 	/// Set the inverse mass (1 / mass).
 	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
-	/// If you change mass, inertia should probably change as well. See MassProperties::ScaleToMass.
+	/// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
 	/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
 	void					SetInverseMass(float inInverseMass)								{ mInvMass = inInverseMass; }
 
@@ -109,10 +109,14 @@ public:
 
 	/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
 	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
-	/// If you change inertia, mass should probably change as well. See MassProperties::ScaleToMass.
+	/// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
 	/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
 	void					SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)			{ mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
 
+	/// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass.
+	/// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted).
+	void					ScaleToMass(float inMass);
+
 	/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
 	inline Mat44			GetLocalSpaceInverseInertia() const;
 

Jolt/Physics/Body/MotionProperties.inl

@@ -50,6 +50,16 @@ inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
 	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
 }
 
+inline void MotionProperties::ScaleToMass(float inMass)
+{
+	JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass");
+	JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero");
+
+	float new_inv_mass = 1.0f / inMass;
+	mInvInertiaDiagonal *= new_inv_mass / mInvMass;
+	mInvMass = new_inv_mass;
+}
+
 inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
 {
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);

UnitTests/Physics/PhysicsTests.cpp

@@ -347,6 +347,14 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(b1.GetMotionProperties()->GetInertiaRotation(), Quat::sIdentity());
 		CHECK_APPROX_EQUAL(b1.GetMotionProperties()->GetInverseInertiaDiagonal(), cExpectedInertiaDiagonal.Reciprocal());
 
+		// Scale the mass and check that the mass and inertia are correct
+		const float cNewMass = 2.0f;
+		b1.GetMotionProperties()->ScaleToMass(cNewMass);
+		const Vec3 cNewExpectedInertiaDiagonal = cNewMass / 12.0f * cSquaredExtents;
+		CHECK_APPROX_EQUAL(b1.GetMotionProperties()->GetInverseMass(), 1.0f / cNewMass);
+		CHECK_APPROX_EQUAL(b1.GetMotionProperties()->GetInertiaRotation(), Quat::sIdentity());
+		CHECK_APPROX_EQUAL(b1.GetMotionProperties()->GetInverseInertiaDiagonal(), cNewExpectedInertiaDiagonal.Reciprocal());
+
 		// Override only the mass
 		const float cOverriddenMass = 13.0f;
 		const Vec3 cOverriddenMassInertiaDiagonal = cOverriddenMass / 12.0f * cSquaredExtents;