When using Body::AddForce to apply gravity, bodies could gain extra energy during elastic collisions. We now cancel added forces in the direction of the contact normal if the body starts in collision to negate this energy gain. (#1890)

See https://github.com/godotengine/godot/issues/115169

.github/workflows/determinism_check.yml

@@ -1,8 +1,8 @@
 name: Determinism Check
 
 env:
-  CONVEX_VS_MESH_HASH: '0xa7348cad585544bf'
-  RAGDOLL_HASH: '0xc392d8f867b0be5b'
+  CONVEX_VS_MESH_HASH: '0x228ccd6931296de5'
+  RAGDOLL_HASH: '0x3cef06e38d6f6b0e'
   PYRAMID_HASH: '0xafd93b295e75e3f6'
   CHARACTER_VIRTUAL_HASH: '0x898879a1a04196bb'
   EMSCRIPTEN_VERSION: 4.0.22

Docs/ReleaseNotes.md

@@ -18,10 +18,11 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 
 ### Bug Fixes
 
-* Made it possible to make a class outside the JPH namespace serializable.
+* Made it possible to make a class outside the `JPH` namespace serializable.
 * `VehicleConstraint`s are automatically disabled when the vehicle body is not in the `PhysicsSystem`.
-* Fixed an issue where a character could get stuck. If the character was teleported inside an area surrounded by slopes that are steeper than mMaxSlopeAngle, the code to stop the constraint solver from ping ponging between two planes didn't work properly.
-* Fixed an issue where collide/cast shape against a triangle would return a hit result with mShape2Face in incorrect winding order. This caused an incorrect normal in the enhanced internal edge removal algorithm. This in turn resulted in objects not settling properly on dense triangle grids.
+* Fixed an issue where a character could get stuck. If the character was teleported inside an area surrounded by slopes that are steeper than `mMaxSlopeAngle`, the code to stop the constraint solver from ping ponging between two planes didn't work properly.
+* Fixed an issue where collide/cast shape against a triangle would return a hit result with `mShape2Face` in incorrect winding order. This caused an incorrect normal in the enhanced internal edge removal algorithm. This in turn resulted in objects not settling properly on dense triangle grids.
+* When using `Body::AddForce` to apply gravity, bodies could gain extra energy during elastic collisions. We now cancel added forces in the direction of the contact normal if the body starts in collision to negate this energy gain.
 
 ## v5.5.0
 

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -46,7 +46,7 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution)
+JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution)
 {
 	JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
 		<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
@@ -58,39 +58,19 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
 	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
 
-	// The gravity is applied in the beginning of the time step. If we get here, there was a collision
-	// at the beginning of the time step, so we've applied too much gravity. This means that our
-	// calculated restitution can be too high, so when we apply restitution, we cancel the added
-	// velocity due to gravity.
-	float gravity_dt_dot_normal;
+	const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+	const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
 
 	// Calculate velocity of collision points
 	Vec3 relative_velocity;
 	if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
-	{
-		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
-		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
 		relative_velocity = mp2->GetPointVelocityCOM(r2) - mp1->GetPointVelocityCOM(r1);
-		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * (mp2->GetGravityFactor() - mp1->GetGravityFactor());
-	}
 	else if constexpr (Type1 != EMotionType::Static)
-	{
-		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
 		relative_velocity = -mp1->GetPointVelocityCOM(r1);
-		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor();
-	}
 	else if constexpr (Type2 != EMotionType::Static)
-	{
-		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
 		relative_velocity = mp2->GetPointVelocityCOM(r2);
-		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp2->GetGravityFactor();
-	}
 	else
-	{
-		JPH_ASSERT(false); // Static vs static makes no sense
-		relative_velocity = Vec3::sZero();
-		gravity_dt_dot_normal = 0.0f;
-	}
+		static_assert(false, "Static vs static makes no sense");
 	float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal);
 
 	// How much the shapes are penetrating (> 0 if penetrating, < 0 if separated)
@@ -114,11 +94,40 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
 		// position rather than from a position where it is touching the other object. This causes the object
 		// to appear to move faster for 1 frame (the opposite of time stealing).
 		if (normal_velocity < -speculative_contact_velocity_bias)
-			normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal);
+		{
+			// The gravity / constant forces are applied in the beginning of the time step.
+			// If we get here, there was a collision at the beginning of the time step, so we've applied too much force.
+			// This means that our calculated restitution can be too high resulting in an increase in energy.
+			// So, when we apply restitution, we cancel the added velocity due to these forces.
+			Vec3 relative_acceleration;
+
+			// Calculate effect of gravity
+			if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+				relative_acceleration = inGravity * (mp2->GetGravityFactor() - mp1->GetGravityFactor());
+			else if constexpr (Type1 != EMotionType::Static)
+				relative_acceleration = -inGravity * mp1->GetGravityFactor();
+			else if constexpr (Type2 != EMotionType::Static)
+				relative_acceleration = inGravity * mp2->GetGravityFactor();
+			else
+				static_assert(false, "Static vs static makes no sense");
+
+			// Calculate effect of accumulated forces
+			if constexpr (Type1 == EMotionType::Dynamic)
+				relative_acceleration -= mp1->GetAccumulatedForce() * mp1->GetInverseMass();
+			if constexpr (Type2 == EMotionType::Dynamic)
+				relative_acceleration += mp2->GetAccumulatedForce() * mp2->GetInverseMass();
+
+			// We only compensate forces towards the contact normal.
+			float force_delta_velocity = min(0.0f, relative_acceleration.Dot(inWorldSpaceNormal) * inDeltaTime);
+
+			normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - force_delta_velocity);
+		}
 		else
+		{
 			// In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities)
 			// the speculative contact will prevent penetration but will not apply restitution leading to another artifact.
 			normal_velocity_bias = speculative_contact_velocity_bias;
+		}
 	}
 	else
 	{
@@ -706,7 +715,7 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
 	// Calculate scaled mass and inertia
 	Mat44 inv_i1;
@@ -737,20 +746,17 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
 
 	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
 
-	// Calculate value for restitution correction
-	float gravity_dt_dot_normal = inGravityDeltaTime.Dot(ws_normal);
-
 	// Setup velocity constraint properties
 	float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution;
 	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
 	{
 		RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
 		RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
-		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, gravity_dt_dot_normal, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
+		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inGravity, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
 	}
 }
 
-inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
 	// Dispatch to the correct templated form
 	switch (inBody1.GetMotionType())
@@ -759,15 +765,15 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
 		switch (inBody2.GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		case EMotionType::Kinematic:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		case EMotionType::Static:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		default:
@@ -778,12 +784,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
 
 	case EMotionType::Kinematic:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	case EMotionType::Static:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	default:
@@ -863,11 +869,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 	RMat44 transform_body1 = body1->GetCenterOfMassTransform();
 	RMat44 transform_body2 = body2->GetCenterOfMassTransform();
 
-	// Get time step
+	// Get time step and gravity
 	float delta_time = mUpdateContext->mStepDeltaTime;
-
-	// Calculate value for restitution correction
-	Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time;
+	Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity();
 
 	// Copy manifolds
 	uint32 output_handle = ManifoldMap::cInvalidHandle;
@@ -970,7 +974,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 			JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
 
 			// Calculate friction and non-penetration constraint properties for all contact points
-			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2);
+			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity, transform_body1, transform_body2, *body1, *body2);
 
 			// Notify island builder
 			mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
@@ -1144,11 +1148,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		// Notify island builder
 		mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal());
 
-		// Get time step
+		// Get time step and gravity
 		float delta_time = mUpdateContext->mStepDeltaTime;
-
-		// Calculate value for restitution correction
-		float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time);
+		Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity();
 
 		// Calculate scaled mass and inertia
 		float inv_m1;
@@ -1222,7 +1224,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			wcp.mContactPoint = &cp;
 
 			// Setup velocity constraint
-			wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, gravity_dt_dot_normal, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution);
+			wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, gravity, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution);
 		}
 
 	#ifdef JPH_DEBUG_RENDERER

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -427,7 +427,7 @@ private:
 		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
 
 		template <EMotionType Type1, EMotionType Type2>
-		JPH_INLINE void			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
+		JPH_INLINE void			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
 
 		/// The constraint parts
 		AxisConstraintPart		mNonPenetrationConstraint;
@@ -485,10 +485,10 @@ public:
 private:
 	/// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>
-	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
 
 	/// Internal helper function to calculate the friction and non-penetration constraint properties.
-	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
 
 	/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
 	template <EMotionType Type1, EMotionType Type2>

UnitTests/Physics/PhysicsTests.cpp

@@ -528,6 +528,7 @@ TEST_SUITE("PhysicsTests")
 	}
 
 	// Let a sphere bounce on the floor with restitution = 1
+	template <bool GravityThroughAddForce>
 	static void TestPhysicsCollisionElastic(PhysicsTestContext &ioContext)
 	{
 		const float cSimulationTime = 1.0f;
@@ -536,19 +537,34 @@ TEST_SUITE("PhysicsTests")
 		const RVec3 cFloorHitPos(0.0f, 1.0f - cFloorHitEpsilon, 0.0f); // Sphere with radius 1 will hit floor when 1 above the floor
 		const RVec3 cInitialPos = cFloorHitPos - cDistanceTraveled;
 
+		// Cancel normal gravity and apply it through AddForce if requested
+		if constexpr (GravityThroughAddForce)
+			ioContext.ZeroGravity();
+
 		// Create sphere
 		ioContext.CreateFloor();
 		Body &body = ioContext.CreateSphere(cInitialPos, 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
 		body.SetRestitution(1.0f);
 
+		// Callback to apply gravity
+		auto apply_custom_gravity = [&body]() {
+				if constexpr (GravityThroughAddForce)
+				{
+					CHECK(body.GetAccumulatedForce() == Vec3::sZero()); // Should have been reset after the step
+					body.AddForce(cGravity / body.GetMotionProperties()->GetInverseMass());
+				}
+				JPH_UNUSED(body);
+			};
+
 		// Simulate until at floor
-		ioContext.Simulate(cSimulationTime);
+		ioContext.Simulate(cSimulationTime, apply_custom_gravity);
 		CHECK_APPROX_EQUAL(cFloorHitPos, body.GetPosition());
 
 		// Assert collision not yet processed
 		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate one more step to process the collision
+		apply_custom_gravity();
 		ioContext.SimulateSingleStep();
 
 		// Assert that collision is processed and velocity is reversed (which is required for a fully elastic collision).
@@ -565,13 +581,14 @@ TEST_SUITE("PhysicsTests")
 
 		// Simulate same time minus one step, with a fully elastic body we should reach the initial position again
 		RVec3 expected_pos = ioContext.PredictPosition(pos_after_bounce_full_step, reflected_velocity_after_full_step, cGravity, cSimulationTime - ioContext.GetDeltaTime());
-		ioContext.Simulate(cSimulationTime - ioContext.GetDeltaTime());
+		ioContext.Simulate(cSimulationTime - ioContext.GetDeltaTime(), apply_custom_gravity);
 		CHECK_APPROX_EQUAL(expected_pos, body.GetPosition(), 1.0e-5f);
 		CHECK_APPROX_EQUAL(expected_pos, cInitialPos, 1.0e-5f);
 
 		// If we do one more step, we should be going down again
 		RVec3 pre_step_pos = body.GetPosition();
 		CHECK(body.GetLinearVelocity().GetY() > 0.0f);
+		apply_custom_gravity();
 		ioContext.SimulateSingleStep();
 		CHECK(body.GetLinearVelocity().GetY() < 1.0e-6f);
 		CHECK(body.GetPosition().GetY() < pre_step_pos.GetY() + 1.0e-6f);
@@ -580,13 +597,22 @@ TEST_SUITE("PhysicsTests")
 	TEST_CASE("TestPhysicsCollisionElastic")
 	{
 		PhysicsTestContext c1(1.0f / 60.0f, 1);
-		TestPhysicsCollisionElastic(c1);
+		TestPhysicsCollisionElastic<false>(c1);
 
 		PhysicsTestContext c2(2.0f / 60.0f, 2);
-		TestPhysicsCollisionElastic(c2);
+		TestPhysicsCollisionElastic<false>(c2);
 
 		PhysicsTestContext c4(4.0f / 60.0f, 4);
-		TestPhysicsCollisionElastic(c4);
+		TestPhysicsCollisionElastic<false>(c4);
+
+		PhysicsTestContext c5(1.0f / 60.0f, 1);
+		TestPhysicsCollisionElastic<true>(c5);
+
+		PhysicsTestContext c6(2.0f / 60.0f, 2);
+		TestPhysicsCollisionElastic<true>(c6);
+
+		PhysicsTestContext c7(4.0f / 60.0f, 4);
+		TestPhysicsCollisionElastic<true>(c7);
 	}
 
 	// Let a sphere with restitution 0.9 bounce on the floor