Added functions to reset force and torque (#546)

See: godot-jolt/godot-jolt#332

Jolt/Physics/Body/Body.cpp

@@ -58,8 +58,8 @@ void Body::SetMotionType(EMotionType inMotionType)
 
 		case EMotionType::Kinematic:
 			// Cancel forces
-			mMotionProperties->mForce = Float3(0, 0, 0);
-			mMotionProperties->mTorque = Float3(0, 0, 0);
+			mMotionProperties->ResetForce();
+			mMotionProperties->ResetTorque();
 			break;
 
 		case EMotionType::Dynamic:

Jolt/Physics/Body/Body.h

@@ -144,6 +144,12 @@ public:
 	// Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSimulation::Update.
 	inline Vec3				GetAccumulatedTorque() const									{ JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); }
 
+	// Reset the total accumulated force, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); }
+
+	// Reset the total accumulated torque, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); }
+
 	/// Get inverse inertia tensor in world space
 	inline Mat44			GetInverseInertia() const;
 

Jolt/Physics/Body/MotionProperties.h

@@ -117,6 +117,12 @@ public:
 	// Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSimulation::Update.
 	JPH_INLINE Vec3			GetAccumulatedTorque() const									{ return Vec3::sLoadFloat3Unsafe(mTorque); }
 
+	// Reset the total accumulated force, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ mForce = Float3(0, 0, 0); }
+
+	// Reset the total accumulated torque, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ mTorque = Float3(0, 0, 0); }
+
 	////////////////////////////////////////////////////////////
 	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
 	////////////////////////////////////////////////////////////
@@ -132,9 +138,6 @@ public:
 	/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
 	inline void				ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
 
-	/// At the end of a simulation update the forces and torques need to be reset for the next frame
-	inline void				ResetForceAndTorqueInternal()									{ mForce = Float3(0, 0, 0); mTorque = Float3(0, 0, 0); }
-
 	/// Access to the island index
 	uint32					GetIslandIndexInternal() const									{ return mIslandIndex; }
 	void					SetIslandIndexInternal(uint32 inIndex)							{ mIslandIndex = inIndex; }

Jolt/Physics/Body/MotionProperties.inl

@@ -101,10 +101,10 @@ void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, V
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
 	// Update linear velocity
-	mLinearVelocity += inDeltaTime * (mGravityFactor * inGravity + mInvMass * Vec3::sLoadFloat3Unsafe(mForce));
+	mLinearVelocity += inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce());
 
 	// Update angular velocity
-	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, Vec3::sLoadFloat3Unsafe(mTorque));
+	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());
 
 	// Linear damping: dv/dt = -c * v
 	// Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt)

Jolt/Physics/PhysicsSystem.cpp

@@ -2234,7 +2234,9 @@ void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const Physic
 			all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mSubStepDeltaTime, max_movement, time_before_sleep));
 
 			// Reset force and torque
-			body.GetMotionProperties()->ResetForceAndTorqueInternal();
+			MotionProperties *mp = body.GetMotionProperties();
+			mp->ResetForce();
+			mp->ResetTorque();
 		}
 
 		// If all bodies indicate they can sleep we can deactivate them