Added interface to BodyInterface to add forces and torques (#87)

Jolt/Physics/Body/Body.h

@@ -121,9 +121,12 @@ public:
 	/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
 	inline Vec3				GetPointVelocity(Vec3Arg inPoint) const							{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return GetPointVelocityCOM(inPoint - mPosition); }
 
-	/// Add force (unit: N) for the next time step, will be reset after the next call to PhysicsSimulation::Update
+	/// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSimulation::Update
 	inline void				AddForce(Vec3Arg inForce)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
 
+	/// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSimulation::Update
+	inline void				AddForce(Vec3Arg inForce, Vec3Arg inPosition);
+
 	/// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSimulation::Update
 	inline void				AddTorque(Vec3Arg inTorque)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
 

Jolt/Physics/Body/Body.inl

@@ -114,6 +114,12 @@ Mat44 Body::GetInverseInertia() const
 	return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
 }
 
+void Body::AddForce(Vec3Arg inForce, Vec3Arg inPosition)
+{
+	AddForce(inForce);
+	AddTorque((inPosition - mPosition).Cross(inForce));
+}
+
 void Body::AddImpulse(Vec3Arg inImpulse)
 {
 	JPH_ASSERT(IsDynamic());

Jolt/Physics/Body/BodyInterface.cpp

@@ -566,6 +566,71 @@ Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, Vec3Arg inPoint) co
 	return Vec3::sZero();
 }
 
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddForce(inForce);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inPoint)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddForce(inForce, inPoint);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddTorque(inTorque);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddForce(inForce);
+			body.AddTorque(inTorque);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
 void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);

Jolt/Physics/Body/BodyInterface.h

@@ -138,6 +138,14 @@ public:
 	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
 	void						SetPositionRotationAndVelocity(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
 
+	///@name Add forces to the body
+	///@{
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce); ///< See Body::AddForce
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inPoint); ///< Applied at inPoint
+	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque); ///< See Body::AddTorque
+	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque); ///< A combination of Body::AddForce and Body::AddTorque
+	///@}
+
 	///@name Add an impulse to the body
 	///@{
 	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass