Added ability to switch motion quality for a body (#342)

* Added ChangeMotionQualityTest

Co-authored-by: Mikael Hermansson <[email protected]>

Jolt/Physics/Body/BodyInterface.cpp

@@ -806,6 +806,22 @@ EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const
 		return EMotionType::Static;
 }
 
+void BodyInterface::SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		mBodyManager->SetMotionQuality(lock.GetBody(), inMotionQuality);
+}
+
+EMotionQuality BodyInterface::GetMotionQuality(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded() && !lock.GetBody().IsStatic())
+		return lock.GetBody().GetMotionProperties()->GetMotionQuality();
+	else
+		return EMotionQuality::Discrete;
+}
+
 Mat44 BodyInterface::GetInverseInertia(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);

Jolt/Physics/Body/BodyInterface.h

@@ -7,6 +7,7 @@
 #include <Jolt/Physics/EActivation.h>
 #include <Jolt/Physics/Collision/ObjectLayer.h>
 #include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
 #include <Jolt/Core/Reference.h>
 
 JPH_NAMESPACE_BEGIN
@@ -197,6 +198,12 @@ public:
 	EMotionType					GetMotionType(const BodyID &inBodyID) const;
 	///@}
 
+	///@name Body motion quality
+	///@{
+	void						SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality);
+	EMotionQuality				GetMotionQuality(const BodyID &inBodyID) const;
+	///@}
+
 	/// Get inverse inertia tensor in world space
 	Mat44						GetInverseInertia(const BodyID &inBodyID) const;
 

Jolt/Physics/Body/BodyManager.cpp

@@ -155,7 +155,7 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 		mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity
 		mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity);
 		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
-		mp->SetMotionQuality(inBodyCreationSettings.mMotionQuality);
+		mp->mMotionQuality = inBodyCreationSettings.mMotionQuality;
 		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
 		mp->mIndexInActiveBodies = Body::cInactiveIndex;
 		mp->mIslandIndex = Body::cInactiveIndex;
@@ -482,6 +482,28 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 		}
 }
 
+void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality)
+{
+	if (ioBody.IsActive())
+	{
+		MotionProperties *mp = ioBody.GetMotionProperties();
+		if (mp->mMotionQuality != inMotionQuality)
+		{
+			UniqueLock lock(mActiveBodiesMutex, EPhysicsLockTypes::ActiveBodiesList);
+
+			JPH_ASSERT(!mActiveBodiesLocked);
+
+			if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+				--mNumActiveCCDBodies;
+
+			mp->mMotionQuality = inMotionQuality;
+
+			if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+				++mNumActiveCCDBodies;
+		}
+	}
+}
+
 void BodyManager::GetActiveBodies(BodyIDVector &outBodyIDs) const
 {
 	JPH_PROFILE_FUNCTION();

Jolt/Physics/Body/BodyManager.h

@@ -86,6 +86,9 @@ public:
 	/// This function should only be called when an exclusive lock for the bodies are held.
 	void							DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
 
+	/// Update the motion quality for a body
+	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
+
 	/// Get copy of the list of active bodies under protection of a lock.
 	void							GetActiveBodies(BodyIDVector &outBodyIDs) const;
 

Jolt/Physics/Body/MotionProperties.h

@@ -22,7 +22,6 @@ public:
 
 	/// Motion quality, or how well it detects collisions when it has a high velocity
 	EMotionQuality			GetMotionQuality() const										{ return mMotionQuality; }
-	void					SetMotionQuality(EMotionQuality inQuality)						{ mMotionQuality = inQuality; }
 
 	/// Get world space linear velocity of the center of mass
 	inline Vec3				GetLinearVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mLinearVelocity; }

Samples/Samples.cmake

@@ -75,6 +75,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/General/BigVsSmallTest.h
 	${SAMPLES_ROOT}/Tests/General/CenterOfMassTest.cpp
 	${SAMPLES_ROOT}/Tests/General/CenterOfMassTest.h
+	${SAMPLES_ROOT}/Tests/General/ChangeMotionQualityTest.cpp
+	${SAMPLES_ROOT}/Tests/General/ChangeMotionQualityTest.h
 	${SAMPLES_ROOT}/Tests/General/ChangeMotionTypeTest.cpp
 	${SAMPLES_ROOT}/Tests/General/ChangeMotionTypeTest.h
 	${SAMPLES_ROOT}/Tests/General/ChangeShapeTest.cpp

Samples/SamplesApp.cpp

@@ -77,6 +77,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(ManifoldReductionTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(CenterOfMassTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(HeavyOnLightTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(HighSpeedTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(ChangeMotionQualityTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(ChangeMotionTypeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(ChangeShapeTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(ChangeObjectLayerTest)
@@ -108,6 +109,7 @@ static TestNameAndRTTI sGeneralTests[] =
 	{ "Center Of Mass",						JPH_RTTI(CenterOfMassTest) },
 	{ "Heavy On Light",						JPH_RTTI(HeavyOnLightTest) },
 	{ "High Speed",							JPH_RTTI(HighSpeedTest) },
+	{ "Change Motion Quality",				JPH_RTTI(ChangeMotionQualityTest) },
 	{ "Change Motion Type",					JPH_RTTI(ChangeMotionTypeTest) },
 	{ "Change Shape",						JPH_RTTI(ChangeShapeTest) },
 	{ "Change Object Layer",				JPH_RTTI(ChangeObjectLayerTest) },

Samples/Tests/General/ChangeMotionQualityTest.cpp

@@ -0,0 +1,67 @@
+#include <TestFramework.h>
+
+#include <Tests/General/ChangeMotionQualityTest.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(ChangeMotionQualityTest)
+{
+	JPH_ADD_BASE_CLASS(ChangeMotionQualityTest, Test)
+}
+
+void ChangeMotionQualityTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Single shape that has 4 walls to surround fast moving sphere
+	BodyCreationSettings enclosing_settings;
+	Ref<BoxShapeSettings> box_shape = new BoxShapeSettings(Vec3(5.0f, 1.0f, 0.1f));
+	Ref<StaticCompoundShapeSettings> enclosing_shape = new StaticCompoundShapeSettings();
+	enclosing_shape->AddShape(Vec3(0, 0, 5), Quat::sIdentity(), box_shape);
+	enclosing_shape->AddShape(Vec3(0, 0, -5), Quat::sIdentity(), box_shape);
+	enclosing_shape->AddShape(Vec3(5, 0, 0), Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI), box_shape);
+	enclosing_shape->AddShape(Vec3(-5, 0, 0), Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI), box_shape);
+	enclosing_settings.SetShapeSettings(enclosing_shape);
+	enclosing_settings.mMotionType = EMotionType::Kinematic;
+	enclosing_settings.mObjectLayer = Layers::MOVING;
+	enclosing_settings.mPosition = Vec3(0, 1, 0);
+	Body &enclosing = *mBodyInterface->CreateBody(enclosing_settings);
+	mBodyInterface->AddBody(enclosing.GetID(), EActivation::Activate);
+
+	// Create high speed sphere inside
+	BodyCreationSettings settings;
+	settings.SetShape(new SphereShape(1.0f));
+	settings.mPosition = Vec3(0, 0.5f, 0);
+	settings.mMotionType = EMotionType::Dynamic;
+	settings.mMotionQuality = EMotionQuality::LinearCast;
+	settings.mLinearVelocity = Vec3(-240, 0, -120);
+	settings.mFriction = 0.0f;
+	settings.mRestitution = 1.0f;
+	settings.mObjectLayer = Layers::MOVING;
+	mBody = mBodyInterface->CreateBody(settings);
+	mBodyInterface->AddBody(mBody->GetID(), EActivation::Activate);
+}
+
+void ChangeMotionQualityTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Increment time
+	mTime += inParams.mDeltaTime;
+
+	// Calculate desired motion quality
+	EMotionQuality motion_quality = (int(mTime) & 1) == 0? EMotionQuality::LinearCast : EMotionQuality::Discrete;
+	mBodyInterface->SetMotionQuality(mBody->GetID(), motion_quality);
+}
+
+void ChangeMotionQualityTest::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mTime);
+}
+
+void ChangeMotionQualityTest::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mTime);
+}

Samples/Tests/General/ChangeMotionQualityTest.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <Tests/Test.h>
+#include <Jolt/Physics/Body/Body.h>
+
+// This test will switch a body's motion quality between discrete and linear
+class ChangeMotionQualityTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(ChangeMotionQualityTest)
+
+	// See: Test
+	virtual void	Initialize() override;
+	virtual void	PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+	virtual void	SaveState(StateRecorder &inStream) const override;
+	virtual void	RestoreState(StateRecorder &inStream) override;
+
+private:
+	Body *			mBody = nullptr;
+	float			mTime = 0.0f;
+};