Helper function to disable parent child collisions (#77)

* Added example on how to create a ragdoll from code

Jolt/Physics/Collision/GroupFilterTable.h

@@ -83,6 +83,14 @@ public:
 		mTable[bit >> 3] |= 1 << (bit & 0b111);
 	}
 
+	/// Check if the collision between two subgroups is enabled
+	inline bool				IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const
+	{
+		// Test if the bit is set for this group pair
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
 	/// Checks if two CollisionGroups collide
 	virtual bool			CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override
 	{	
@@ -102,9 +110,8 @@ public:
 		if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID())
 			return false;
 
-		// Test if the bit is set for this group pair
-		int bit = GetBit(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID());
-		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+		// Check the bit table
+		return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID());
 	}
 
 	// See: GroupFilter::SaveBinaryState

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -7,6 +7,10 @@
 #include <Physics/Ragdoll/Ragdoll.h>
 #include <Physics/PhysicsSystem.h>
 #include <Physics/Body/BodyLockMulti.h>
+#include <Physics/Collision/GroupFilterTable.h>
+#include <Physics/Collision/CollisionCollectorImpl.h>
+#include <Physics/Collision/CollideShape.h>
+#include <Physics/Collision/CollisionDispatch.h>
 #include <ObjectStream/TypeDeclarations.h>
 #include <Core/StreamIn.h>
 #include <Core/StreamOut.h>
@@ -177,6 +181,73 @@ bool RagdollSettings::Stabilize()
 	return true;
 }
 
+void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance)
+{
+	int joint_count = mSkeleton->GetJointCount();
+	JPH_ASSERT(joint_count == (int)mParts.size());
+
+	// Create a group filter table that disables collisions between parent and child
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(joint_count);
+	for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx)
+	{
+		int parent_joint = mSkeleton->GetJoint(joint_idx).mParentJointIndex;
+		if (parent_joint >= 0)
+			group_filter->DisableCollision(joint_idx, parent_joint);
+	}
+
+	// If joint matrices are provided
+	if (inJointMatrices != nullptr)
+	{
+		// Loop over all joints
+		for (int j1 = 0; j1 < joint_count; ++j1)
+		{
+			// Shape and transform for joint 1
+			const Part &part1 = mParts[j1];
+			const Shape *shape1 = part1.GetShape();
+			Vec3 scale1;
+			Mat44 com1 = (inJointMatrices[j1] * Mat44::sTranslation(shape1->GetCenterOfMass())).Decompose(scale1);
+
+			// Loop over all other joints
+			for (int j2 = j1 + 1; j2 < joint_count; ++j2)
+				if (group_filter->IsCollisionEnabled(j1, j2)) // Only if collision is still enabled we need to test
+				{
+					// Shape and transform for joint 2
+					const Part &part2 = mParts[j2];
+					const Shape *shape2 = part2.GetShape();
+					Vec3 scale2;
+					Mat44 com2 = (inJointMatrices[j2] * Mat44::sTranslation(shape2->GetCenterOfMass())).Decompose(scale2);
+					
+					// Collision settings
+					CollideShapeSettings settings;
+					settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+					settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+					settings.mMaxSeparationDistance = inMinSeparationDistance;
+
+					// Only check if one of the two bodies can become dynamic
+					if (part1.HasMassProperties() || part2.HasMassProperties())
+					{
+						// If there is a collision, disable the collision between the joints
+						AnyHitCollisionCollector<CollideShapeCollector> collector;
+						if (part1.HasMassProperties()) // Ensure that the first shape is always a dynamic one (we can't check mesh vs convex but we can check convex vs mesh)
+							CollisionDispatch::sCollideShapeVsShape(shape1, shape2, scale1, scale2, com1, com2, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+						else
+							CollisionDispatch::sCollideShapeVsShape(shape2, shape1, scale2, scale1, com2, com1, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+						if (collector.HadHit())
+							group_filter->DisableCollision(j1, j2);
+					}
+				}
+		}
+	}
+
+	// Loop over the body parts and assign them a sub group ID and the group filter
+	for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx)
+	{
+		Part &part = mParts[joint_idx];
+		part.mCollisionGroup.SetSubGroupID(joint_idx);
+		part.mCollisionGroup.SetGroupFilter(group_filter);
+	}
+}
+
 void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const
 {
 	BodyCreationSettings::ShapeToIDMap shape_to_id;

Jolt/Physics/Ragdoll/Ragdoll.h

@@ -26,6 +26,20 @@ public:
 	/// @return True on success, false on failure.
 	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. 
+	/// 
+	/// 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 
+	/// (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 
+	/// 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);
+
 	/// Saves the state of this object in binary form to inStream.
 	/// @param inStream The stream to save the state to
 	/// @param inSaveShapes If the shapes should be saved as well (these could be shared between ragdolls, in which case the calling application may want to write custom code to restore them)
@@ -113,7 +127,7 @@ public:
 	/// Set the group ID on all bodies in the ragdoll
 	void								SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies = true);
 
-	/// Set the ragdoll to a pose (calls PhysicsSystem::SetPositionAndRotation to instantly move the ragdoll)
+	/// 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

Jolt/Skeleton/Skeleton.h

@@ -40,6 +40,8 @@ public:
 	int						GetJointCount() const													{ return (int)mJoints.size(); }
 	const Joint &			GetJoint(int inJoint) const												{ return mJoints[inJoint]; }
 	Joint &					GetJoint(int inJoint)													{ return mJoints[inJoint]; }
+	uint					AddJoint(const string &inName, const string &inParentName = string())	{ mJoints.push_back({ inName, inParentName, -1 }); return (uint)mJoints.size() - 1; }
+	uint					AddJoint(const string &inName, int inParentIndex)						{ mJoints.push_back({ inName, inParentIndex >= 0? mJoints[inParentIndex].mName : string(), inParentIndex }); return (uint)mJoints.size() - 1; }
 	///@}
 
 	/// Find joint by name

Samples/Samples.cmake

@@ -113,6 +113,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/General/WallTest.h
 	${SAMPLES_ROOT}/Tests/General/ActivateDuringUpdateTest.cpp
 	${SAMPLES_ROOT}/Tests/General/ActivateDuringUpdateTest.h
+	${SAMPLES_ROOT}/Tests/Rig/CreateRigTest.cpp
+	${SAMPLES_ROOT}/Tests/Rig/CreateRigTest.h
 	${SAMPLES_ROOT}/Tests/Rig/KinematicRigTest.cpp
 	${SAMPLES_ROOT}/Tests/Rig/KinematicRigTest.h
 	${SAMPLES_ROOT}/Tests/Rig/LoadSaveBinaryRigTest.cpp

Samples/SamplesApp.cpp

@@ -205,6 +205,7 @@ static TestNameAndRTTI sScaledShapeTests[] =
 	{ "Offset Center Of Mass Shape",		JPH_RTTI(ScaledOffsetCenterOfMassShapeTest) }
 };
 
+JPH_DECLARE_RTTI_FOR_FACTORY(CreateRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(LoadRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(KinematicRigTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(PoweredRigTest)
@@ -213,6 +214,7 @@ JPH_DECLARE_RTTI_FOR_FACTORY(LoadSaveBinaryRigTest)
 
 static TestNameAndRTTI sRigTests[] =
 {
+	{ "Create Rig",							JPH_RTTI(CreateRigTest) },
 	{ "Load Rig",							JPH_RTTI(LoadRigTest) },
 	{ "Load / Save Binary Rig",				JPH_RTTI(LoadSaveBinaryRigTest) },
 	{ "Kinematic Rig",						JPH_RTTI(KinematicRigTest) },

Samples/Tests/Rig/CreateRigTest.cpp

@@ -0,0 +1,206 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/Rig/CreateRigTest.h>
+#include <Physics/Collision/Shape/CapsuleShape.h>
+#include <Physics/Constraints/SwingTwistConstraint.h>
+#include <Application/DebugUI.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(CreateRigTest)
+{
+	JPH_ADD_BASE_CLASS(CreateRigTest, Test)
+}
+
+CreateRigTest::~CreateRigTest()
+{
+	mRagdoll->RemoveFromPhysicsSystem();
+}
+
+void CreateRigTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Create skeleton
+	Ref<Skeleton> skeleton = new Skeleton;
+	uint lower_body = skeleton->AddJoint("LowerBody");
+	uint mid_body = skeleton->AddJoint("MidBody", lower_body);
+	uint upper_body = skeleton->AddJoint("UpperBody", mid_body);
+	/*uint head =*/ skeleton->AddJoint("Head", upper_body);
+	uint upper_arm_l = skeleton->AddJoint("UpperArmL", upper_body);
+	uint upper_arm_r = skeleton->AddJoint("UpperArmR", upper_body);
+	/*uint lower_arm_l =*/ skeleton->AddJoint("LowerArmL", upper_arm_l);
+	/*uint lower_arm_r =*/ skeleton->AddJoint("LowerArmR", upper_arm_r);
+	uint upper_leg_l = skeleton->AddJoint("UpperLegL", lower_body);
+	uint upper_leg_r = skeleton->AddJoint("UpperLegR", lower_body);
+	/*uint lower_leg_l =*/ skeleton->AddJoint("LowerLegL", upper_leg_l);
+	/*uint lower_leg_r =*/ skeleton->AddJoint("LowerLegR", upper_leg_r);
+
+	// Create shapes for limbs
+	Ref<Shape> shapes[] = {
+		new CapsuleShape(0.15f, 0.10f),		// Lower Body
+		new CapsuleShape(0.15f, 0.10f),		// Mid Body
+		new CapsuleShape(0.15f, 0.10f),		// Upper Body
+		new CapsuleShape(0.075f, 0.10f),	// Head
+		new CapsuleShape(0.15f, 0.06f),		// Upper Arm L
+		new CapsuleShape(0.15f, 0.06f),		// Upper Arm R
+		new CapsuleShape(0.15f, 0.05f),		// Lower Arm L
+		new CapsuleShape(0.15f, 0.05f),		// Lower Arm R
+		new CapsuleShape(0.2f, 0.075f),		// Upper Leg L
+		new CapsuleShape(0.2f, 0.075f),		// Upper Leg R
+		new CapsuleShape(0.2f, 0.06f),		// Lower Leg L
+		new CapsuleShape(0.2f, 0.06f),		// Lower Leg R
+	};
+
+	// Positions of body parts in world space
+	Vec3 positions[] = {
+		Vec3(0, 1.15f, 0),					// Lower Body
+		Vec3(0, 1.35f, 0),					// Mid Body
+		Vec3(0, 1.55f, 0),					// Upper Body
+		Vec3(0, 1.825f, 0),					// Head
+		Vec3(-0.425f, 1.55f, 0),			// Upper Arm L
+		Vec3(0.425f, 1.55f, 0),				// Upper Arm R
+		Vec3(-0.8f, 1.55f, 0),				// Lower Arm L
+		Vec3(0.8f, 1.55f, 0),				// Lower Arm R
+		Vec3(-0.15f, 0.8f, 0),				// Upper Leg L
+		Vec3(0.15f, 0.8f, 0),				// Upper Leg R
+		Vec3(-0.15f, 0.3f, 0),				// Lower Leg L
+		Vec3(0.15f, 0.3f, 0),				// Lower Leg R
+	};
+
+	// Rotations of body parts in world space
+	Quat rotations[] = {
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Lower Body
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Mid Body
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Upper Body
+		Quat::sIdentity(),									 // Head
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Upper Arm L
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Upper Arm R
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Lower Arm L
+		Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI),		 // Lower Arm R
+		Quat::sIdentity(),									 // Upper Leg L
+		Quat::sIdentity(),									 // Upper Leg R
+		Quat::sIdentity(),									 // Lower Leg L
+		Quat::sIdentity()									 // Lower Leg R
+	};
+
+	// World space constraint positions
+	Vec3 constraint_positions[] = {
+		Vec3::sZero(),				// Lower Body (unused, there's no parent)
+		Vec3(0, 1.25f, 0),			// Mid Body
+		Vec3(0, 1.45f, 0),			// Upper Body
+		Vec3(0, 1.65f, 0),			// Head
+		Vec3(-0.225f, 1.55f, 0),	// Upper Arm L
+		Vec3(0.225f, 1.55f, 0),		// Upper Arm R
+		Vec3(-0.65f, 1.55f, 0),		// Lower Arm L
+		Vec3(0.65f, 1.55f, 0),		// Lower Arm R
+		Vec3(-0.15f, 1.05f, 0),		// Upper Leg L
+		Vec3(0.15f, 1.05f, 0),		// Upper Leg R
+		Vec3(-0.15f, 0.55f, 0),		// Lower Leg L
+		Vec3(0.15f, 0.55f, 0),		// Lower Leg R
+	};
+
+	// World space twist axis directions
+	Vec3 twist_axis[] = {
+		Vec3::sZero(),				// Lower Body (unused, there's no parent)
+		Vec3::sAxisY(),				// Mid Body
+		Vec3::sAxisY(),				// Upper Body
+		Vec3::sAxisY(),				// Head
+		-Vec3::sAxisX(),			// Upper Arm L
+		Vec3::sAxisX(),				// Upper Arm R
+		-Vec3::sAxisX(),			// Lower Arm L
+		Vec3::sAxisX(),				// Lower Arm R
+		-Vec3::sAxisY(),			// Upper Leg L
+		-Vec3::sAxisY(),			// Upper Leg R
+		-Vec3::sAxisY(),			// Lower Leg L
+		-Vec3::sAxisY(),			// Lower Leg R
+	};
+
+	// Constraint limits
+	float twist_angle[] = {
+		0.0f,		// Lower Body (unused, there's no parent)
+		5.0f,		// Mid Body
+		5.0f,		// Upper Body
+		90.0f,		// Head
+		45.0f,		// Upper Arm L
+		45.0f,		// Upper Arm R
+		45.0f,		// Lower Arm L
+		45.0f,		// Lower Arm R
+		45.0f,		// Upper Leg L
+		45.0f,		// Upper Leg R
+		45.0f,		// Lower Leg L
+		45.0f,		// Lower Leg R
+	};
+
+	float normal_angle[] = {
+		0.0f,		// Lower Body (unused, there's no parent)
+		10.0f,		// Mid Body
+		10.0f,		// Upper Body
+		45.0f,		// Head
+		90.0f,		// Upper Arm L
+		90.0f,		// Upper Arm R
+		0.0f,		// Lower Arm L
+		0.0f,		// Lower Arm R
+		45.0f,		// Upper Leg L
+		45.0f,		// Upper Leg R
+		0.0f,		// Lower Leg L
+		0.0f,		// Lower Leg R
+	};
+
+	float plane_angle[] = {
+		0.0f,		// Lower Body (unused, there's no parent)
+		10.0f,		// Mid Body
+		10.0f,		// Upper Body
+		45.0f,		// Head
+		45.0f,		// Upper Arm L
+		45.0f,		// Upper Arm R
+		90.0f,		// Lower Arm L
+		90.0f,		// Lower Arm R
+		45.0f,		// Upper Leg L
+		45.0f,		// Upper Leg R
+		60.0f,		// Lower Leg L (cheating here, a knee is not symmetric, we should have rotated the twist axis)
+		60.0f,		// Lower Leg R
+	};
+
+	// Create ragdoll settings
+	Ref<RagdollSettings> settings = new RagdollSettings;
+	settings->mSkeleton = skeleton;
+	settings->mParts.resize(skeleton->GetJointCount());
+	for (int p = 0; p < skeleton->GetJointCount(); ++p)
+	{
+		RagdollSettings::Part &part = settings->mParts[p];
+		part.SetShape(shapes[p]);
+		part.mPosition = positions[p];
+		part.mRotation = rotations[p];
+		part.mMotionType = EMotionType::Dynamic;
+		part.mObjectLayer = Layers::MOVING;
+
+		// First part is the root, doesn't have a parent and doesn't have a constraint
+		if (p > 0)
+		{
+			SwingTwistConstraintSettings *constraint = new SwingTwistConstraintSettings;
+			constraint->mDrawConstraintSize = 0.1f;
+			constraint->mPosition1 = constraint->mPosition2 = constraint_positions[p];
+			constraint->mTwistAxis1 = constraint->mTwistAxis2 = twist_axis[p];
+			constraint->mPlaneAxis1 = constraint->mPlaneAxis2 = Vec3::sAxisZ();
+			constraint->mTwistMinAngle = -DegreesToRadians(twist_angle[p]);
+			constraint->mTwistMaxAngle = DegreesToRadians(twist_angle[p]);
+			constraint->mNormalHalfConeAngle = DegreesToRadians(normal_angle[p]);
+			constraint->mPlaneHalfConeAngle = DegreesToRadians(plane_angle[p]);
+			part.mToParent = constraint;
+		}
+	}
+
+	// Optional: Stabilize the inertia of the limbs
+	settings->Stabilize();
+
+	// Disable parent child collisions so that we don't get collisions between constrained bodies
+	settings->DisableParentChildCollisions();
+
+	// Create ragdoll
+	mRagdoll = settings->CreateRagdoll(0, 0, mPhysicsSystem);
+	mRagdoll->AddToPhysicsSystem(EActivation::Activate);
+}

Samples/Tests/Rig/CreateRigTest.h

@@ -0,0 +1,26 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+#include <Physics/Ragdoll/Ragdoll.h>
+
+// This test demonstrates how to create a ragdoll from code
+class CreateRigTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(CreateRigTest)
+
+	// Destructor
+	virtual							~CreateRigTest() 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;
+};