Added support for gyroscopic forces (#809)

Fixes #803

Jolt/Physics/Body/Body.cpp

@@ -336,6 +336,7 @@ BodyCreationSettings Body::GetBodyCreationSettings() const
 	result.mIsSensor = IsSensor();
 	result.mSensorDetectsStatic = SensorDetectsStatic();
 	result.mUseManifoldReduction = GetUseManifoldReduction();
+	result.mApplyGyroscopicForce = GetApplyGyroscopicForce();
 	result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete;
 	result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true;
 	result.mFriction = GetFriction();

Jolt/Physics/Body/Body.h

@@ -97,6 +97,12 @@ class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public
 	/// Checks if the combination of this body and inBody2 should use manifold reduction
 	inline bool				GetUseManifoldReductionWithBody(const Body &inBody2) const		{ return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; }
 
+	/// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				SetApplyGyroscopicForce(bool inApply)							{ JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); }
+
+	/// Check if the gyroscopic force is being applied for this body
+	inline bool				GetApplyGyroscopicForce() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; }
+
 	/// Get the bodies motion type.
 	inline EMotionType		GetMotionType() const											{ return mMotionType; }
 
@@ -326,6 +332,7 @@ class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public
 		IsInBroadPhase			= 1 << 2,													///< Set this bit to indicate that the body is in the broadphase
 		InvalidateContactCache	= 1 << 3,													///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
 		UseManifoldReduction	= 1 << 4,													///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
+		ApplyGyroscopicForce	= 1 << 5,													///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
 	};
 
 	// 16 byte aligned

Jolt/Physics/Body/BodyCreationSettings.cpp

@@ -27,6 +27,7 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mSensorDetectsStatic)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce)
 	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping)
 	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction)
@@ -57,6 +58,7 @@ void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mIsSensor);
 	inStream.Write(mSensorDetectsStatic);
 	inStream.Write(mUseManifoldReduction);
+	inStream.Write(mApplyGyroscopicForce);
 	inStream.Write(mMotionQuality);
 	inStream.Write(mAllowSleeping);
 	inStream.Write(mFriction);
@@ -87,6 +89,7 @@ void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mIsSensor);
 	inStream.Read(mSensorDetectsStatic);
 	inStream.Read(mUseManifoldReduction);
+	inStream.Read(mApplyGyroscopicForce);
 	inStream.Read(mMotionQuality);
 	inStream.Read(mAllowSleeping);
 	inStream.Read(mFriction);

Jolt/Physics/Body/BodyCreationSettings.h

@@ -96,6 +96,7 @@ class JPH_EXPORT BodyCreationSettings
 	bool					mIsSensor = false;												///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
 	bool					mSensorDetectsStatic = false;									///< If this sensor detects static objects entering it. Note that the sensor must be kinematic and active for it to detect static objects.
 	bool					mUseManifoldReduction = true;									///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction)
+	bool					mApplyGyroscopicForce = false;									///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
 	EMotionQuality			mMotionQuality = EMotionQuality::Discrete;						///< Motion quality, or how well it detects collisions when it has a high velocity
 	bool					mAllowSleeping = true;											///< If this body can go to sleep or not
 	float					mFriction = 0.2f;												///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.

Jolt/Physics/Body/BodyManager.cpp

@@ -202,6 +202,8 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 		body->SetSensorDetectsStatic(true);
 	if (inBodyCreationSettings.mUseManifoldReduction)
 		body->SetUseManifoldReduction(true);
+	if (inBodyCreationSettings.mApplyGyroscopicForce)
+		body->SetApplyGyroscopicForce(true);
 	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);
 	body->mObjectLayer = inBodyCreationSettings.mObjectLayer;
 	body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup;

Jolt/Physics/Body/MotionProperties.h

@@ -168,6 +168,9 @@ class JPH_EXPORT MotionProperties
 	inline void				SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) 		{ JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
 	///@}
 
+	/// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
+
 	/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
 	inline void				ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
 

Jolt/Physics/Body/MotionProperties.inl

@@ -73,6 +73,34 @@ Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRo
 	return rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(inV));
 }
 
+void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Calculate local space inertia tensor (a diagonal in local space)
+	UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
+	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero);
+	Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero);
+	Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
+
+	// Calculate local space angular momentum
+	Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation;
+	Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity;
+	Vec3 local_momentum = local_inertia * local_angular_velocity;
+
+	// The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor
+	// Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one
+	// to avoid introducing energy into the system due to the Euler step
+	Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum);
+	float new_local_momentum_len_sq = new_local_momentum.LengthSq();
+	new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero();
+
+	// Convert back to world space angular velocity
+	mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum);
+}
+
 void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));

Jolt/Physics/PhysicsSystem.cpp

@@ -701,7 +701,15 @@ void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, Physi
 		{
 			Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
 			if (body.IsDynamic())
-				body.GetMotionProperties()->ApplyForceTorqueAndDragInternal(body.GetRotation(), mGravity, delta_time);
+			{
+				MotionProperties *mp = body.GetMotionProperties();
+				Quat rotation = body.GetRotation();
+
+				if (body.GetApplyGyroscopicForce())
+					mp->ApplyGyroscopicForceInternal(rotation, delta_time);
+
+				mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time);
+			}
 			active_body_idx++;
 		}
 	}

Samples/Samples.cmake

@@ -111,6 +111,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/General/FunnelTest.h
 	${SAMPLES_ROOT}/Tests/General/GravityFactorTest.cpp
 	${SAMPLES_ROOT}/Tests/General/GravityFactorTest.h
+	${SAMPLES_ROOT}/Tests/General/GyroscopicForceTest.cpp
+	${SAMPLES_ROOT}/Tests/General/GyroscopicForceTest.h
 	${SAMPLES_ROOT}/Tests/General/HeavyOnLightTest.cpp
 	${SAMPLES_ROOT}/Tests/General/HeavyOnLightTest.h
 	${SAMPLES_ROOT}/Tests/General/HighSpeedTest.cpp

Samples/SamplesApp.cpp

@@ -102,6 +102,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, DynamicMeshTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, TwoDFunnelTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, AllowedDOFsTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, ShapeFilterTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, GyroscopicForceTest)
 
 static TestNameAndRTTI sGeneralTests[] =
 {
@@ -140,6 +141,7 @@ static TestNameAndRTTI sGeneralTests[] =
 	{ "Dynamic Mesh",						JPH_RTTI(DynamicMeshTest) },
 	{ "Allowed Degrees of Freedom",			JPH_RTTI(AllowedDOFsTest) },
 	{ "Shape Filter",						JPH_RTTI(ShapeFilterTest) },
+	{ "Gyroscopic Force",					JPH_RTTI(GyroscopicForceTest) },
 };
 
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, DistanceConstraintTest)

Samples/Tests/General/GyroscopicForceTest.cpp

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/General/GyroscopicForceTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(GyroscopicForceTest)
+{
+	JPH_ADD_BASE_CLASS(GyroscopicForceTest, Test)
+}
+
+void GyroscopicForceTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	StaticCompoundShapeSettings compound;
+	compound.AddShape(Vec3::sZero(), Quat::sIdentity(), new BoxShape(Vec3(0.5f, 5.0f, 0.5f)));
+	compound.AddShape(Vec3(1.5f, 0, 0), Quat::sIdentity(), new BoxShape(Vec3(1.0f, 0.5f, 0.5f)));
+	compound.SetEmbedded();
+
+	// One body without gyroscopic force
+	BodyCreationSettings bcs(&compound, RVec3(0, 10, 0), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mLinearDamping = 0.0f;
+	bcs.mAngularDamping = 0.0f;
+	bcs.mAngularVelocity = Vec3(10, 1, 0);
+	bcs.mGravityFactor = 0.0f;
+	mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+
+	// One body with gyroscopic force
+	bcs.mPosition += RVec3(10, 0, 0);
+	bcs.mApplyGyroscopicForce = true;
+	mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+}

Samples/Tests/General/GyroscopicForceTest.h

@@ -0,0 +1,18 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/BodyActivationListener.h>
+
+// This shows how to enable gyrosopic forces to create the Dzhanibekov effect (see: https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+class GyroscopicForceTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, GyroscopicForceTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};