Fixed bodies gaining more energy than intended due to restitution (#1328)

Fixes #1286

Co-authored-by: @mutexmiles

.github/workflows/determinism_check.yml

@@ -1,8 +1,8 @@
 name: Determinism Check
 
 env:
-  CONVEX_VS_MESH_HASH: '0x10139effe747511'
-  RAGDOLL_HASH: '0x777396947c3fff6a'
+  CONVEX_VS_MESH_HASH: '0x16ca5bf7f9da5f74'
+  RAGDOLL_HASH: '0xf33d6c177e59f69'
   EMSCRIPTEN_VERSION: 3.1.64
   UBUNTU_CLANG_VERSION: clang++-15
   UBUNTU_GCC_VERSION: g++-12

Jolt/Physics/Constraints/ContactConstraintManager.cpp

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/PhysicsUpdateContext.h>
 #include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/IslandBuilder.h>
 #include <Jolt/Physics/DeterminismLog.h>
 #include <Jolt/Core/TempAllocator.h>
@@ -45,7 +46,7 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, 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, 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_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
 		<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
@@ -57,18 +58,38 @@ 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;
+
 	// Calculate velocity of collision points
 	Vec3 relative_velocity;
 	if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
-		relative_velocity = inBody2.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r2) - inBody1.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r1);
+	{
+		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)
-		relative_velocity = -inBody1.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r1);
+	{
+		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+		relative_velocity = -mp1->GetPointVelocityCOM(r1);
+		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor();
+	}
 	else if constexpr (Type2 != EMotionType::Static)
-		relative_velocity = inBody2.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r2);
+	{
+		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;
 	}
 	float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal);
 
@@ -93,7 +114,7 @@ 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;
+			normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal);
 		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.
@@ -676,7 +697,7 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
 }
 
 template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
 	// Calculate scaled mass and inertia
 	Mat44 inv_i1;
@@ -707,17 +728,20 @@ 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, 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, 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);
 	}
 }
 
-inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
 {
 	// Dispatch to the correct templated form
 	switch (inBody1.GetMotionType())
@@ -726,15 +750,15 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
 		switch (inBody2.GetMotionType())
 		{
 		case EMotionType::Dynamic:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		case EMotionType::Kinematic:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		case EMotionType::Static:
-			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 			break;
 
 		default:
@@ -745,12 +769,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
 
 	case EMotionType::Kinematic:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	case EMotionType::Static:
 		JPH_ASSERT(inBody2.IsDynamic());
-		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
 		break;
 
 	default:
@@ -835,6 +859,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
 	// Get time step
 	float delta_time = mUpdateContext->mStepDeltaTime;
 
+	// Calculate value for restitution correction
+	Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time;
+
 	// Copy manifolds
 	uint32 output_handle = ManifoldMap::cInvalidHandle;
 	uint32 input_handle = input_cbp.mFirstCachedManifold;
@@ -936,7 +963,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, transform_body1, transform_body2, *body1, *body2);
+			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2);
 
 			// Notify island builder
 			mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
@@ -1107,6 +1134,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 		// Get time step
 		float delta_time = mUpdateContext->mStepDeltaTime;
 
+		// Calculate value for restitution correction
+		float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time);
+
 		// Calculate scaled mass and inertia
 		float inv_m1;
 		Mat44 inv_i1;
@@ -1179,7 +1209,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
 			wcp.mContactPoint = &cp;
 
 			// Setup velocity constraint
-			wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, 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_dt_dot_normal, 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

@@ -424,7 +424,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, 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, 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);
 
 		/// The constraint parts
 		AxisConstraintPart		mNonPenetrationConstraint;
@@ -474,10 +474,10 @@ 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, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, 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, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, 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

@@ -401,17 +401,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsFreeFall")
 	{
-		PhysicsTestContext c;
-		TestPhysicsFreeFall(c);
-	}
-
-	TEST_CASE("TestPhysicsFreeFallStep")
-	{
-		PhysicsTestContext c1(2.0f / 60.0f, 2);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
 		TestPhysicsFreeFall(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsFreeFall(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsFreeFall(c4);
 	}
 
 	// Test acceleration of a box with force applied
@@ -443,17 +440,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsApplyForce")
 	{
-		PhysicsTestContext c;
-		TestPhysicsApplyForce(c);
-	}
-
-	TEST_CASE("TestPhysicsApplyForceStep")
-	{
-		PhysicsTestContext c1(2.0f / 60.0f, 2);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
 		TestPhysicsApplyForce(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsApplyForce(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsApplyForce(c4);
 	}
 
 	// Test angular acceleration for a box by applying torque every frame
@@ -487,17 +481,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsApplyTorque")
 	{
-		PhysicsTestContext c;
-		TestPhysicsApplyTorque(c);
-	}
-
-	TEST_CASE("TestPhysicsApplyTorqueStep")
-	{
-		PhysicsTestContext c1(2.0f / 60.0f, 2);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
 		TestPhysicsApplyTorque(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsApplyTorque(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsApplyTorque(c4);
 	}
 
 	// Let a sphere bounce on the floor with restitution = 1
@@ -522,15 +513,12 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate one more step to process the collision
-		ioContext.Simulate(ioContext.GetDeltaTime());
+		ioContext.SimulateSingleStep();
 
 		// Assert that collision is processed and velocity is reversed (which is required for a fully elastic collision).
-		// Note that the physics engine will first apply gravity for the time step and then do collision detection,
-		// hence the reflected velocity is actually 1 step times gravity bigger than it would be in reality
-		// For the remainder of cDeltaTime normal gravity will be applied
 		float sub_step_delta_time = ioContext.GetStepDeltaTime();
 		float remaining_step_time = ioContext.GetDeltaTime() - ioContext.GetStepDeltaTime();
-		Vec3 reflected_velocity_after_sub_step = -(cSimulationTime + sub_step_delta_time) * cGravity;
+		Vec3 reflected_velocity_after_sub_step = -cSimulationTime * cGravity;
 		Vec3 reflected_velocity_after_full_step = reflected_velocity_after_sub_step + remaining_step_time * cGravity;
 		CHECK_APPROX_EQUAL(reflected_velocity_after_full_step, body.GetLinearVelocity(), 1.0e-4f);
 
@@ -539,27 +527,100 @@ TEST_SUITE("PhysicsTests")
 		RVec3 pos_after_bounce_full_step = ioContext.PredictPosition(pos_after_bounce_sub_step, reflected_velocity_after_sub_step, cGravity, remaining_step_time);
 		CHECK_APPROX_EQUAL(pos_after_bounce_full_step, body.GetPosition());
 
-		// Simulate same time, with a fully elastic body we should reach the initial position again
-		// In our physics engine because of the velocity being too big we actually end up a bit higher than our initial position
-		RVec3 expected_pos = ioContext.PredictPosition(pos_after_bounce_full_step, reflected_velocity_after_full_step, cGravity, cSimulationTime);
-		ioContext.Simulate(cSimulationTime);
-		CHECK_APPROX_EQUAL(expected_pos, body.GetPosition(), 1.0e-4f);
-		CHECK(expected_pos.GetY() >= cInitialPos.GetY());
+		// 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());
+		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);
+		ioContext.SimulateSingleStep();
+		CHECK(body.GetLinearVelocity().GetY() < 1.0e-6f);
+		CHECK(body.GetPosition().GetY() < pre_step_pos.GetY() + 1.0e-6f);
 	}
 
 	TEST_CASE("TestPhysicsCollisionElastic")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsCollisionElastic(c1);
+
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
+		TestPhysicsCollisionElastic(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsCollisionElastic(c4);
+	}
+
+	// Let a sphere with restitution 0.9 bounce on the floor
+	TEST_CASE("TestPhysicsCollisionPartiallyElastic")
 	{
 		PhysicsTestContext c;
-		TestPhysicsCollisionElastic(c);
+		c.CreateFloor();
+
+		// Create sphere
+		const RVec3 cInitialPos(0, 10, 0);
+		constexpr float cRestitution = 0.9f;
+		constexpr float cRadius = 2.0f;
+		Body &body = c.CreateSphere(cInitialPos, cRadius, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		body.SetRestitution(cRestitution);
+
+		// Simple simulation to compare with the actual simulation
+		RVec3 pos = cInitialPos;
+		Vec3 vel = Vec3::sZero();
+		float dt = c.GetDeltaTime();
+		float penetration_slop = c.GetSystem()->GetPhysicsSettings().mPenetrationSlop;
+		for (int i = 0; i < 1000; ++i)
+		{
+			// Simple simulation
+			Real penetration = cRadius - pos.GetY();
+			if (penetration > -penetration_slop && vel.GetY() < 0.0f)
+				vel = -cRestitution * vel;
+			else
+				vel += cGravity * dt;
+			pos += vel * dt;
+
+			// Actual step
+			c.SimulateSingleStep();
+
+			// Compare simulations
+			CHECK_APPROX_EQUAL(pos, body.GetPosition(), 1.0e-5f);
+			CHECK_APPROX_EQUAL(vel, body.GetLinearVelocity(), 1.0e-5f);
+		}
 	}
 
-	TEST_CASE("TestPhysicsCollisionElasticStep")
+	// 2 spheres bounce with restitution = 1, tests we don't correct for gravity in a perpendicular direction to gravity
+	static void TestPhysicsCollisionElasticDynamic(PhysicsTestContext &ioContext)
 	{
-		PhysicsTestContext c1(2.0f / 60.0f, 2);
-		TestPhysicsCollisionElastic(c1);
+		// Create spheres
+		Body &sphere1 = ioContext.CreateSphere(RVec3(-2, 0, 0), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		sphere1.SetRestitution(1.0f);
+		sphere1.SetLinearVelocity(Vec3(5, 0, 0));
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
-		TestPhysicsCollisionElastic(c2);
+		Body &sphere2 = ioContext.CreateSphere(RVec3(2, 0, 0), 1.0f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
+		sphere2.SetRestitution(1.0f);
+		sphere2.SetLinearVelocity(Vec3(-10, 0, 0));
+
+		// Simulate
+		constexpr float cSimulationTime = 1.0f;
+		ioContext.Simulate(cSimulationTime);
+
+		// Check that velocities match that of a fully elastic collision
+		CHECK_APPROX_EQUAL(Vec3(-10, 0, 0) + cSimulationTime * cGravity, sphere1.GetLinearVelocity(), 1.0e-5f);
+		CHECK_APPROX_EQUAL(Vec3(5, 0, 0) + cSimulationTime * cGravity, sphere2.GetLinearVelocity(), 1.0e-5f);
+	}
+
+	TEST_CASE("TestPhysicsCollisionElasticDynamic")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsCollisionElasticDynamic(c1);
+
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
+		TestPhysicsCollisionElasticDynamic(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsCollisionElasticDynamic(c4);
 	}
 
 	// Let a sphere bounce on the floor with restitution = 0
@@ -584,7 +645,7 @@ TEST_SUITE("PhysicsTests")
 		CHECK_APPROX_EQUAL(cSimulationTime * cGravity, body.GetLinearVelocity(), 1.0e-4f);
 
 		// Simulate one more step to process the collision
-		ioContext.Simulate(ioContext.GetDeltaTime());
+		ioContext.SimulateSingleStep();
 
 		// Assert that all velocity was lost in the collision
 		CHECK_APPROX_EQUAL(Vec3::sZero(), body.GetLinearVelocity(), 1.0e-4f);
@@ -600,17 +661,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsCollisionInelastic")
 	{
-		PhysicsTestContext c;
-		TestPhysicsCollisionInelastic(c);
-	}
-
-	TEST_CASE("TestPhysicsCollisionInelasticStep")
-	{
-		PhysicsTestContext c1(2.0f / 60.0f, 2);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
 		TestPhysicsCollisionInelastic(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsCollisionInelastic(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsCollisionInelastic(c4);
 	}
 
 	// Let box intersect with floor by cPenetrationSlop. It should not move, this is the maximum penetration allowed.
@@ -635,17 +693,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsPenetrationSlop1")
 	{
-		PhysicsTestContext c;
-		TestPhysicsPenetrationSlop1(c);
-	}
-
-	TEST_CASE("TestPhysicsPenetrationSlop1Step")
-	{
-		PhysicsTestContext c(2.0f / 60.0f, 2);
-		TestPhysicsPenetrationSlop1(c);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsPenetrationSlop1(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsPenetrationSlop1(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsPenetrationSlop1(c4);
 	}
 
 	// Let box intersect with floor with more than cPenetrationSlop. It should be resolved by SolvePositionConstraint until interpenetration is cPenetrationSlop.
@@ -671,17 +726,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsPenetrationSlop2")
 	{
-		PhysicsTestContext c;
-		TestPhysicsPenetrationSlop2(c);
-	}
-
-	TEST_CASE("TestPhysicsPenetrationSlop2Step")
-	{
-		PhysicsTestContext c(2.0f / 60.0f, 2);
-		TestPhysicsPenetrationSlop2(c);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsPenetrationSlop2(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsPenetrationSlop2(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsPenetrationSlop2(c4);
 	}
 
 	// Let box intersect with floor with less than cPenetrationSlop. Body should not move because SolveVelocityConstraint should reset velocity.
@@ -706,17 +758,14 @@ TEST_SUITE("PhysicsTests")
 
 	TEST_CASE("TestPhysicsPenetrationSlop3")
 	{
-		PhysicsTestContext c;
-		TestPhysicsPenetrationSlop3(c);
-	}
-
-	TEST_CASE("TestPhysicsPenetrationSlop3Step")
-	{
-		PhysicsTestContext c(2.0f / 60.0f, 2);
-		TestPhysicsPenetrationSlop3(c);
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsPenetrationSlop3(c1);
 
-		PhysicsTestContext c2(4.0f / 60.0f, 4);
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsPenetrationSlop3(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsPenetrationSlop3(c4);
 	}
 
 	TEST_CASE("TestPhysicsOutsideOfSpeculativeContactDistance")
@@ -1091,8 +1140,8 @@ TEST_SUITE("PhysicsTests")
 		PhysicsTestContext c2(2.0f / 60.0f, 2);
 		TestPhysicsActivationDeactivation(c2);
 
-		PhysicsTestContext c3(4.0f / 60.0f, 4);
-		TestPhysicsActivationDeactivation(c3);
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsActivationDeactivation(c4);
 	}
 
 	// A test that checks that a row of penetrating boxes will all activate and handle collision in 1 frame so that active bodies cannot tunnel through inactive bodies