From 2bbc4e024414bacf742e67a89518415ad47e66f2 Mon Sep 17 00:00:00 2001
From: Paul Sinnett <paul.sinnett@gmail.com>
Date: Wed, 28 Aug 2024 21:24:50 +0100
Subject: [PATCH] added leapfrog integration

leapfrog integration applies half of the first acceleration update to velocity on the first frame of waking. After that, the updates proceed as usual. This corrects impulses and makes them work as expected rather than losing 5% of the energy after the first frame.
---
 physx/source/lowlevel/software/include/PxsRigidBody.h |  2 ++
 physx/source/simulationcontroller/src/ScBodySim.cpp   |  7 +++++--
 physx/source/simulationcontroller/src/ScPipeline.cpp  | 11 +++++++++++
 physx/source/simulationcontroller/src/ScScene.cpp     |  2 ++
 4 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/physx/source/lowlevel/software/include/PxsRigidBody.h b/physx/source/lowlevel/software/include/PxsRigidBody.h
index 650ab1e4a..67eeee3cd 100644
--- a/physx/source/lowlevel/software/include/PxsRigidBody.h
+++ b/physx/source/lowlevel/software/include/PxsRigidBody.h
@@ -123,6 +123,8 @@ class PxsRigidBody
 	PX_FORCE_INLINE	void				clearFreezeFlag()							{ mInternalFlags &= ~eFREEZE_THIS_FRAME;					}
 	PX_FORCE_INLINE	void				clearUnfreezeFlag()							{ mInternalFlags &= ~eUNFREEZE_THIS_FRAME;					}
 	PX_FORCE_INLINE	void				clearAllFrameFlags()						{ mInternalFlags &= ~(eFREEZE_THIS_FRAME | eUNFREEZE_THIS_FRAME | eACTIVATE_THIS_FRAME | eDEACTIVATE_THIS_FRAME);	}
+	PX_FORCE_INLINE void				setLeapfrogAccelerationScale()				{ mAccelScale = 0.5f;										}
+	PX_FORCE_INLINE void				resetLeapfrogAccelerationScale()			{ mAccelScale = 1.0f;										}
 
 	PX_FORCE_INLINE	void				resetSleepFilter()							{ mSleepAngVelAcc = mSleepLinVelAcc = PxVec3(0.0f);			}
 
diff --git a/physx/source/simulationcontroller/src/ScBodySim.cpp b/physx/source/simulationcontroller/src/ScBodySim.cpp
index 7fab5a325..a63c16e47 100644
--- a/physx/source/simulationcontroller/src/ScBodySim.cpp
+++ b/physx/source/simulationcontroller/src/ScBodySim.cpp
@@ -670,11 +670,14 @@ bool BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* upd
 	if( (accDirty || velDirty) && ((simStateData = getSimStateData(false)) != NULL) )
 	{
 		VelocityMod* velmod = simStateData->getVelocityModData();
+		PxReal accelScale = 1.f;
 
 		//we don't have support for articulation yet
 		if (updatedBodySims)
 		{
-			updatedBodySims[index] = &getLowLevelBody();
+			PxsRigidBody& body = getLowLevelBody();
+			accelScale = body.mAccelScale;
+			updatedBodySims[index] = &body;
 			updatedBodyNodeIndices[index++] = getNodeIndex().index();
 		}
 
@@ -686,7 +689,7 @@ bool BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* upd
 		
 		if (accDirty)
 		{
-			linVelDt += velmod->getLinearVelModPerSec()*dt;
+			linVelDt += velmod->getLinearVelModPerSec()*dt*accelScale;
 			angVelDt += velmod->getAngularVelModPerSec()*dt;
 		}
 
diff --git a/physx/source/simulationcontroller/src/ScPipeline.cpp b/physx/source/simulationcontroller/src/ScPipeline.cpp
index 12cc87b11..dd6f13fd1 100644
--- a/physx/source/simulationcontroller/src/ScPipeline.cpp
+++ b/physx/source/simulationcontroller/src/ScPipeline.cpp
@@ -2277,6 +2277,17 @@ void Sc::Scene::afterIntegration(PxBaseTask* continuation)
 
 		const IG::IslandSim& islandSim = mSimpleIslandManager->getAccurateIslandSim();
 
+		{
+			// clear leapfrog acceleration scale for all active bodies
+			const PxU32 nbActiveBodies = islandSim.getNbActiveNodes(IG::Node::eRIGID_BODY_TYPE);
+			const PxNodeIndex*const activeBodies = islandSim.getActiveNodes(IG::Node::eRIGID_BODY_TYPE);
+			for (PxU32 i = 0; i < nbActiveBodies; ++i)
+			{
+				PxsRigidBody* body = islandSim.getRigidBody(activeBodies[i]);
+				body->resetLeapfrogAccelerationScale();
+			}
+		}
+
 		const PxU32 rigidBodyOffset = BodySim::getRigidBodyOffset();
 
 		const PxU32 numBodiesToDeactivate = islandSim.getNbNodesToDeactivate(IG::Node::eRIGID_BODY_TYPE);
diff --git a/physx/source/simulationcontroller/src/ScScene.cpp b/physx/source/simulationcontroller/src/ScScene.cpp
index 674dd6a89..fe609da53 100644
--- a/physx/source/simulationcontroller/src/ScScene.cpp
+++ b/physx/source/simulationcontroller/src/ScScene.cpp
@@ -2765,6 +2765,8 @@ void Sc::Scene::onBodySleep(BodySim* body)
 
 void Sc::Scene::onBodyWakeUp(BodySim* body)
 {
+	body->getLowLevelBody().setLeapfrogAccelerationScale();
+
 	if(!mSimulationEventCallback && !mOnSleepingStateChanged)
 		return;