Added ability to specify extra ragdoll constraints that are not parent/child related (#677)

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -25,10 +25,17 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part)
 	JPH_ADD_ATTRIBUTE(RagdollSettings::Part, mToParent)
 }
 
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::AdditionalConstraint)
+{
+	JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mBodyIdx)
+	JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mConstraint)
+}
+
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings)
 {
 	JPH_ADD_ATTRIBUTE(RagdollSettings, mSkeleton)
 	JPH_ADD_ATTRIBUTE(RagdollSettings, mParts)
+	JPH_ADD_ATTRIBUTE(RagdollSettings, mAdditionalConstraints)
 }
 
 static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
@@ -61,7 +68,7 @@ bool RagdollSettings::Stabilize()
 		// Mark static bodies as visited so we won't process these
 		Part &p = mParts[v];
 		bool has_mass_properties = p.HasMassProperties();
-		visited[v] = !has_mass_properties; 
+		visited[v] = !has_mass_properties;
 
 		if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided)
 		{
@@ -83,7 +90,7 @@ bool RagdollSettings::Stabilize()
 			indices.reserve(mSkeleton->GetJointCount());
 			visited[first_idx] = true;
 			indices.push_back(first_idx);
-			do 
+			do
 			{
 				int parent_idx = indices[next_to_process++];
 				for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx)
@@ -100,7 +107,7 @@ bool RagdollSettings::Stabilize()
 
 			const float cMinMassRatio = 0.8f;
 			const float cMaxMassRatio = 1.2f;
-	
+
 			// Ensure that the mass ratio from parent to child is within a range
 			float total_mass_ratio = 1.0f;
 			Array<float> mass_ratios;
@@ -142,7 +149,7 @@ bool RagdollSettings::Stabilize()
 				Mat44	mRotation;
 				Vec3	mDiagonal;
 				float	mChildSum = 0.0f;
-			};	
+			};
 			Array<Principal> principals;
 			principals.resize(mParts.size());
 			for (int i : indices)
@@ -217,7 +224,7 @@ void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices,
 					const Shape *shape2 = part2.GetShape();
 					Vec3 scale2;
 					Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2);
-					
+
 					// Collision settings
 					CollideShapeSettings settings;
 					settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
@@ -270,6 +277,17 @@ void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bo
 		if (p.mToParent != nullptr)
 			p.mToParent->SaveBinaryState(inStream);
 	}
+
+	// Save additional constraints
+	inStream.Write((uint32)mAdditionalConstraints.size());
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+	{
+		// Save bodies indices
+		inStream.Write(c.mBodyIdx);
+
+		// Save constraint
+		c.mConstraint->SaveBinaryState(inStream);
+	}
 }
 
 RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn &inStream)
@@ -327,6 +345,25 @@ RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn
 		}
 	}
 
+	// Read additional constraints
+	len = 0;
+	inStream.Read(len);
+	ragdoll->mAdditionalConstraints.resize(len);
+	for (AdditionalConstraint &c : ragdoll->mAdditionalConstraints)
+	{
+		// Read body indices
+		inStream.Read(c.mBodyIdx);
+
+		// Read constraint
+		ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
+		if (constraint_result.HasError())
+		{
+			result.SetError(constraint_result.GetError());
+			return result;
+		}
+		c.mConstraint = DynamicCast<TwoBodyConstraintSettings>(constraint_result.Get().GetPtr());
+	}
+
 	// Create mapping tables
 	ragdoll->CalculateBodyIndexToConstraintIndex();
 	ragdoll->CalculateConstraintIndexToBodyIdxPair();
@@ -340,8 +377,9 @@ Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup
 	Ragdoll *r = new Ragdoll(inSystem);
 	r->mRagdollSettings = this;
 	r->mBodyIDs.reserve(mParts.size());
-	r->mConstraints.reserve(mParts.size());
+	r->mConstraints.reserve(mParts.size() + mAdditionalConstraints.size());
 
+	// Create bodies and constraints
 	BodyInterface &bi = inSystem->GetBodyInterface();
 	Body **bodies = (Body **)JPH_STACK_ALLOC(mParts.size() * sizeof(Body *));
 	int joint_idx = 0;
@@ -373,6 +411,14 @@ Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup
 		++joint_idx;
 	}
 
+	// Add additional constraints
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+	{
+		Body *body1 = bodies[c.mBodyIdx[0]];
+		Body *body2 = bodies[c.mBodyIdx[1]];
+		r->mConstraints.push_back(c.mConstraint->Create(*body1, *body2));
+	}
+
 	return r;
 }
 
@@ -394,7 +440,9 @@ void RagdollSettings::CalculateBodyIndexToConstraintIndex()
 void RagdollSettings::CalculateConstraintIndexToBodyIdxPair()
 {
 	mConstraintIndexToBodyIdxPair.clear();
+	mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size());
 
+	// Add constraints between parts
 	int joint_idx = 0;
 	for (const Part &p : mParts)
 	{
@@ -406,10 +454,14 @@ void RagdollSettings::CalculateConstraintIndexToBodyIdxPair()
 
 		++joint_idx;
 	}
+
+	// Add additional constraints
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+		mConstraintIndexToBodyIdxPair.push_back(BodyIdxPair(c.mBodyIdx[0], c.mBodyIdx[1]));
 }
 
 Ragdoll::~Ragdoll()
-{	
+{
 	// Destroy all bodies
 	mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size());
 }

Jolt/Physics/Ragdoll/Ragdoll.h

@@ -28,16 +28,16 @@ public:
 	bool								Stabilize();
 
 	/// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies
-	/// and configure them so that parent and children don't collide. 
-	/// 
+	/// and configure them so that parent and children don't collide.
+	///
 	/// This will:
 	/// - Create a GroupFilterTable and assign it to all of the bodies in a ragdoll.
 	/// - Each body in your ragdoll will get a SubGroupID that is equal to the joint index in the Skeleton that it is attached to.
 	/// - Loop over all joints in the Skeleton and call GroupFilterTable::DisableCollision(joint index, parent joint index).
-	/// - When a pose is provided through inJointMatrices the function will detect collisions between joints 
+	/// - When a pose is provided through inJointMatrices the function will detect collisions between joints
 	/// (they must be separated by more than inMinSeparationDistance to be treated as not colliding) and automatically disable collisions.
-	/// 
-	/// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number 
+	///
+	/// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number
 	/// should be unique throughout the PhysicsSystem, so if you have different types of ragdolls they should not share the same GroupID.
 	void								DisableParentChildCollisions(const Mat44 *inJointMatrices = nullptr, float inMinSeparationDistance = 0.0f);
 
@@ -64,6 +64,7 @@ public:
 	void								CalculateBodyIndexToConstraintIndex();
 
 	/// Get table that maps a body index to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body.
+	/// Note that this will only tell you which constraint connects the body to its parent, it will not look in the additional constraint list.
 	const Array<int> &					GetBodyIndexToConstraintIndex() const							{ return mBodyIndexToConstraintIndex; }
 
 	/// Map a single body index to a constraint index
@@ -92,12 +93,32 @@ public:
 	/// List of ragdoll parts
 	using PartVector = Array<Part>;																	///< The constraint that connects this part to its parent part (should be null for the root)
 
+	/// A constraint that connects two bodies in a ragdoll (for non parent child related constraints)
+	class AdditionalConstraint
+	{
+	public:
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint)
+
+		/// Constructors
+										AdditionalConstraint() = default;
+										AdditionalConstraint(int inBodyIdx1, int inBodyIdx2, TwoBodyConstraintSettings *inConstraint) : mBodyIdx { inBodyIdx1, inBodyIdx2 }, mConstraint(inConstraint) { }
+
+		int								mBodyIdx[2];												///< Indices of the bodies that this constraint connects
+		Ref<TwoBodyConstraintSettings>	mConstraint;												///< The constraint that connects these bodies
+	};
+
+	/// List of additional constraints
+	using AdditionalConstraintVector = Array<AdditionalConstraint>;
+
 	/// The skeleton for this ragdoll
 	Ref<Skeleton>						mSkeleton;
 
 	/// For each of the joints, the body and constraint attaching it to its parent body (1-on-1 with mSkeleton.GetJoints())
 	PartVector							mParts;
 
+	/// A list of constraints that connects two bodies in a ragdoll (for non parent child related constraints)
+	AdditionalConstraintVector			mAdditionalConstraints;
+
 private:
 	/// Table that maps a body index (index in mBodyIDs) to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body.
 	Array<int>							mBodyIndexToConstraintIndex;
@@ -138,7 +159,7 @@ public:
 
 	/// Set the ragdoll to a pose (calls BodyInterface::SetPositionAndRotation to instantly move the ragdoll)
 	void								SetPose(const SkeletonPose &inPose, bool inLockBodies = true);
-	
+
 	/// Lower level version of SetPose that directly takes the world space joint matrices
 	void								SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies = true);
 
@@ -150,7 +171,7 @@ public:
 
 	/// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime
 	void								DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true);
-	
+
 	/// Lower level version of DriveToPoseUsingKinematics that directly takes the world space joint matrices
 	void								DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies = true);
 
@@ -159,10 +180,10 @@ public:
 
 	/// Control the linear and velocity of all bodies in the ragdoll
 	void								SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true);
-	
+
 	/// Set the world space linear velocity of all bodies in the ragdoll.
 	void								SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
-	
+
 	/// Add a world space velocity (in m/s) to all bodies in the ragdoll.
 	void								AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
 

Samples/Samples.cmake

@@ -153,6 +153,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Rig/KinematicRigTest.h
 	${SAMPLES_ROOT}/Tests/Rig/LoadSaveBinaryRigTest.cpp
 	${SAMPLES_ROOT}/Tests/Rig/LoadSaveBinaryRigTest.h
+	${SAMPLES_ROOT}/Tests/Rig/LoadSaveRigTest.cpp
+	${SAMPLES_ROOT}/Tests/Rig/LoadSaveRigTest.h
 	${SAMPLES_ROOT}/Tests/Rig/LoadRigTest.cpp
 	${SAMPLES_ROOT}/Tests/Rig/LoadRigTest.h
 	${SAMPLES_ROOT}/Tests/Rig/PoweredRigTest.cpp

Samples/SamplesApp.cpp

@@ -252,6 +252,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, LoadRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, KinematicRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, PoweredRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, RigPileTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, LoadSaveRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, LoadSaveBinaryRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SkeletonMapperTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BigWorldTest)
@@ -260,6 +261,7 @@ static TestNameAndRTTI sRigTests[] =
 {
 	{ "Create Rig",							JPH_RTTI(CreateRigTest) },
 	{ "Load Rig",							JPH_RTTI(LoadRigTest) },
+	{ "Load / Save Rig",					JPH_RTTI(LoadSaveRigTest) },
 	{ "Load / Save Binary Rig",				JPH_RTTI(LoadSaveBinaryRigTest) },
 	{ "Kinematic Rig",						JPH_RTTI(KinematicRigTest) },
 	{ "Powered Rig",						JPH_RTTI(PoweredRigTest) },

Samples/Tests/Rig/LoadSaveBinaryRigTest.cpp

@@ -5,8 +5,8 @@
 #include <TestFramework.h>
 
 #include <Tests/Rig/LoadSaveBinaryRigTest.h>
-#include <Application/DebugUI.h>
 #include <Jolt/Core/StreamWrapper.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
 #include <Utils/Log.h>
 #include <Utils/RagdollLoader.h>
 
@@ -31,6 +31,16 @@ void LoadSaveBinaryRigTest::Initialize()
 		// Load ragdoll
 		Ref<RagdollSettings> settings = RagdollLoader::sLoad("Assets/Human.tof", EMotionType::Dynamic);
 
+		// Add an additional constraint between the left and right arm to test loading/saving of additional constraints
+		const Skeleton *skeleton = settings->GetSkeleton();
+		int left_arm = skeleton->GetJointIndex("L_Wrist_sjnt_0");
+		int right_arm = skeleton->GetJointIndex("R_Wrist_sjnt_0");
+		Ref<DistanceConstraintSettings> constraint = new DistanceConstraintSettings;
+		constraint->mSpace = EConstraintSpace::LocalToBodyCOM;
+		constraint->mMaxDistance = 0.1f;
+		constraint->mMinDistance = 0.1f;
+		settings->mAdditionalConstraints.push_back(RagdollSettings::AdditionalConstraint(left_arm, right_arm , constraint));
+
 		// Save it to a binary stream
 		StreamOutWrapper stream_out(data);
 		settings->SaveBinaryState(stream_out, true /* Save shape */, true /* Save group filter */);

Samples/Tests/Rig/LoadSaveRigTest.cpp

@@ -0,0 +1,61 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Rig/LoadSaveRigTest.h>
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/ObjectStream/ObjectStreamOut.h>
+#include <Jolt/ObjectStream/ObjectStreamIn.h>
+#include <Utils/Log.h>
+#include <Utils/RagdollLoader.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(LoadSaveRigTest)
+{
+	JPH_ADD_BASE_CLASS(LoadSaveRigTest, Test)
+}
+
+LoadSaveRigTest::~LoadSaveRigTest()
+{
+	mRagdoll->RemoveFromPhysicsSystem();
+}
+
+void LoadSaveRigTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	stringstream data;
+
+	{
+		// Load ragdoll
+		Ref<RagdollSettings> settings = RagdollLoader::sLoad("Assets/Human.tof", EMotionType::Dynamic);
+
+		// Add an additional constraint between the left and right arm to test loading/saving of additional constraints
+		const Skeleton *skeleton = settings->GetSkeleton();
+		int left_arm = skeleton->GetJointIndex("L_Wrist_sjnt_0");
+		int right_arm = skeleton->GetJointIndex("R_Wrist_sjnt_0");
+		Ref<DistanceConstraintSettings> constraint = new DistanceConstraintSettings;
+		constraint->mSpace = EConstraintSpace::LocalToBodyCOM;
+		constraint->mMaxDistance = 0.1f;
+		constraint->mMinDistance = 0.1f;
+		settings->mAdditionalConstraints.push_back(RagdollSettings::AdditionalConstraint(left_arm, right_arm , constraint));
+
+		// Write ragdoll
+		if (!ObjectStreamOut::sWriteObject(data, ObjectStream::EStreamType::Text, *settings))
+			FatalError("Failed to save ragdoll");
+	}
+
+	// Read ragdoll back in
+	Ref<RagdollSettings> settings;
+	if (!ObjectStreamIn::sReadObject(data, settings))
+		FatalError("Failed to load ragdoll");
+
+	// Parent joint indices are not stored so need to be calculated again
+	settings->GetSkeleton()->CalculateParentJointIndices();
+
+	// Create ragdoll
+	mRagdoll = settings->CreateRagdoll(0, 0, mPhysicsSystem);
+	mRagdoll->AddToPhysicsSystem(EActivation::Activate);
+}

Samples/Tests/Rig/LoadSaveRigTest.h

@@ -0,0 +1,27 @@
+// 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/Ragdoll/Ragdoll.h>
+
+// This test loads a ragdoll from disc, writes it to an object stream, loads it again and simulates it
+class LoadSaveRigTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, LoadSaveRigTest)
+
+	// Destructor
+	virtual							~LoadSaveRigTest() override;
+
+	// Number used to scale the terrain and camera movement to the scene
+	virtual float					GetWorldScale() const override								{ return 0.2f; }
+
+	virtual void					Initialize() override;
+
+private:
+	// Our ragdoll
+	Ref<Ragdoll>					mRagdoll;
+};