Bugfix: Warm start ratio was used every sub step but should only be used in the first one (#234)

This causes the simulation the behave erratically when changing the amount of collision/integration steps on the fly.

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -847,7 +847,7 @@ void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhase
 	StoreActiveContacts(contacts, inAllocator);
 }
 
-void CharacterVirtual::MoveToContact(const Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator)
+void CharacterVirtual::MoveToContact(Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator)
 {
 	// Set the new position
 	SetPosition(inPosition);

Jolt/Physics/Character/CharacterVirtual.h

@@ -325,7 +325,7 @@ private:
 	void								UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator);
 
 	/// This function can be called after moving the character to a new colliding position
-	void								MoveToContact(const Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
+	void								MoveToContact(Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
 
 	// This function returns the actual center of mass of the shape, not corrected for the character padding
 	inline Mat44						GetCenterOfMassTransform(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const

Jolt/Physics/PhysicsSystem.cpp

@@ -415,6 +415,7 @@ void PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, int inIntegr
 				sub_step.mStep = &step;
 				sub_step.mIsFirst = is_first_sub_step;
 				sub_step.mIsLast = is_last_sub_step;
+				sub_step.mIsFirstOfAll = is_first_step && is_first_sub_step;
 				sub_step.mIsLastOfAll = is_last_step && is_last_sub_step;
 
 				// This job will solve the velocity constraints 
@@ -1251,12 +1252,14 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 #endif
 	
 	float delta_time = ioContext->mSubStepDeltaTime;
-	float warm_start_impulse_ratio = ioContext->mWarmStartImpulseRatio;
 	Constraint **active_constraints = ioContext->mActiveConstraints;
 
 	bool first_sub_step = ioSubStep->mIsFirst;
 	bool last_sub_step = ioSubStep->mIsLast;
 
+	// Only the first sub step of the first step needs to correct for the delta time difference in the previous update
+	float warm_start_impulse_ratio = ioSubStep->mIsFirstOfAll? ioContext->mWarmStartImpulseRatio : 1.0f; 
+
 	for (;;)
 	{
 		// Next island

Jolt/Physics/PhysicsUpdateContext.h

@@ -39,6 +39,7 @@ public:
 
 		bool				mIsFirst;												///< If this is the first substep in the step
 		bool				mIsLast;												///< If this is the last substep in the step
+		bool				mIsFirstOfAll;											///< If this is the first substep of the first step
 		bool				mIsLastOfAll;											///< If this is the last substep in the last step
 
 		atomic<uint32>		mSolveVelocityConstraintsNextIsland { 0 };				///< Next island that needs to be processed for the solve velocity constraints step (doesn't need own cache line since position jobs don't run at same time)