@bloomengine/engine 0.4.0 → 0.4.1

package/native/third_party/JoltPhysics/Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -1,744 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Ragdoll/Ragdoll.h>
-#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
-#include <Jolt/Physics/Constraints/HingeConstraint.h>
-#include <Jolt/Physics/PhysicsSystem.h>
-#include <Jolt/Physics/Body/BodyLockMulti.h>
-#include <Jolt/Physics/Collision/GroupFilterTable.h>
-#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
-#include <Jolt/Physics/Collision/CollideShape.h>
-#include <Jolt/Physics/Collision/CollisionDispatch.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part)
-{
-	JPH_ADD_BASE_CLASS(RagdollSettings::Part, BodyCreationSettings)
-
-	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 &sRagdollGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
-{
-	return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock();
-}
-
-static inline const BodyLockInterface &sRagdollGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies)
-{
-	return inLockBodies? static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterface()) : static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterfaceNoLock());
-}
-
-bool RagdollSettings::Stabilize()
-{
-	// Based on: Stop my Constraints from Blowing Up! - Oliver Strunk (Havok)
-	// Do 2 things:
-	// 1. Limit the mass ratios between parents and children (slide 16)
-	// 2. Increase the inertia of parents so that they're bigger or equal to the sum of their children (slide 34)
-
-	// If we don't have any joints there's nothing to stabilize
-	if (mSkeleton->GetJointCount() == 0)
-		return true;
-
-	// The skeleton can contain one or more static bodies. We can't modify the mass for those so we start a new stabilization chain for each joint under a static body until we reach the next static body.
-	// This array keeps track of which joints have been processed.
-	Array<bool> visited;
-	visited.resize(mSkeleton->GetJointCount());
-	for (size_t v = 0; v < visited.size(); ++v)
-	{
-		// 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;
-
-		if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided)
-		{
-			// Mass properties not yet calculated, do it now
-			p.mMassPropertiesOverride = p.GetMassProperties();
-			p.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
-		}
-	}
-
-	// Find first unvisited part that either has no parent or that has a parent that was visited
-	for (int first_idx = 0; first_idx < mSkeleton->GetJointCount(); ++first_idx)
-	{
-		int first_idx_parent = mSkeleton->GetJoint(first_idx).mParentJointIndex;
-		if (!visited[first_idx] && (first_idx_parent == -1 || visited[first_idx_parent]))
-		{
-			// Find all children of first_idx and their children up to the next static part
-			int next_to_process = 0;
-			Array<int> indices;
-			indices.reserve(mSkeleton->GetJointCount());
-			visited[first_idx] = true;
-			indices.push_back(first_idx);
-			do
-			{
-				int parent_idx = indices[next_to_process++];
-				for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx)
-					if (!visited[child_idx] && mSkeleton->GetJoint(child_idx).mParentJointIndex == parent_idx)
-					{
-						visited[child_idx] = true;
-						indices.push_back(child_idx);
-					}
-			} while (next_to_process < (int)indices.size());
-
-			// If there's only 1 body, we can't redistribute mass
-			if (indices.size() == 1)
-				continue;
-
-			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;
-			mass_ratios.resize(mSkeleton->GetJointCount());
-			mass_ratios[indices[0]] = 1.0f;
-			for (int i = 1; i < (int)indices.size(); ++i)
-			{
-				int child_idx = indices[i];
-				int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex;
-				float ratio = mParts[child_idx].mMassPropertiesOverride.mMass / mParts[parent_idx].mMassPropertiesOverride.mMass;
-				mass_ratios[child_idx] = mass_ratios[parent_idx] * Clamp(ratio, cMinMassRatio, cMaxMassRatio);
-				total_mass_ratio += mass_ratios[child_idx];
-			}
-
-			// Calculate total mass of this chain
-			float total_mass = 0.0f;
-			for (int idx : indices)
-				total_mass += mParts[idx].mMassPropertiesOverride.mMass;
-
-			// Calculate how much mass belongs to a ratio of 1
-			float ratio_to_mass = total_mass / total_mass_ratio;
-
-			// Adjust all masses and inertia tensors for the new mass
-			for (int i : indices)
-			{
-				Part &p = mParts[i];
-				float old_mass = p.mMassPropertiesOverride.mMass;
-				float new_mass = mass_ratios[i] * ratio_to_mass;
-				p.mMassPropertiesOverride.mMass = new_mass;
-				p.mMassPropertiesOverride.mInertia *= new_mass / old_mass;
-				p.mMassPropertiesOverride.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
-			}
-
-			const float cMaxInertiaIncrease = 2.0f;
-
-			// Get the principal moments of inertia for all parts
-			struct Principal
-			{
-				Mat44	mRotation;
-				Vec3	mDiagonal;
-				float	mChildSum = 0.0f;
-			};
-			Array<Principal> principals;
-			principals.resize(mParts.size());
-			for (int i : indices)
-				if (!mParts[i].mMassPropertiesOverride.DecomposePrincipalMomentsOfInertia(principals[i].mRotation, principals[i].mDiagonal))
-				{
-					JPH_ASSERT(false, "Failed to decompose the inertia tensor!");
-					return false;
-				}
-
-			// Calculate sum of child inertias
-			// Walk backwards so we sum the leaves first
-			for (int i = (int)indices.size() - 1; i > 0; --i)
-			{
-				int child_idx = indices[i];
-				int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex;
-				principals[parent_idx].mChildSum += principals[child_idx].mDiagonal[0] + principals[child_idx].mChildSum;
-			}
-
-			// Adjust inertia tensors for all parts
-			for (int i : indices)
-			{
-				Part &p = mParts[i];
-				Principal &principal = principals[i];
-				if (principal.mChildSum != 0.0f)
-				{
-					// Calculate minimum inertia this object should have based on it children
-					float minimum = min(cMaxInertiaIncrease * principal.mDiagonal[0], principal.mChildSum);
-					principal.mDiagonal = Vec3::sMax(principal.mDiagonal, Vec3::sReplicate(minimum));
-
-					// Recalculate moment of inertia in body space
-					p.mMassPropertiesOverride.mInertia = principal.mRotation * Mat44::sScale(principal.mDiagonal) * principal.mRotation.Inversed3x3();
-				}
-			}
-		}
-	}
-
-	return true;
-}
-
-void RagdollSettings::CalculateConstraintPriorities(uint32 inBasePriority)
-{
-	JPH_ASSERT(inBasePriority + (uint32)mParts.size() > inBasePriority, "Base priority is too high and will cause overflows");
-	JPH_ASSERT(mSkeleton->AreJointsCorrectlyOrdered());
-
-	// Calculate priority for each part. Start with the base priority and increment towards the root
-	Array<uint32> priorities;
-	priorities.resize(mParts.size(), inBasePriority);
-	for (int i = (int)mParts.size() - 1; i >= 0; --i)
-	{
-		uint32 cur_priority = inBasePriority;
-		int j = i;
-		do
-		{
-			priorities[j] = max(priorities[j], cur_priority);
-			cur_priority++;
-
-			j = mSkeleton->GetJoint(j).mParentJointIndex;
-		}
-		while (j != -1);
-	}
-
-	// Copy the priorities to the constraints
-	for (uint i = 0, n = (uint)mParts.size(); i < n; ++i)
-		if (mParts[i].mToParent != nullptr)
-			mParts[i].mToParent->mConstraintPriority = priorities[i];
-
-	// Use the minimum of the priorities of connected bodies for additional constraints
-	for (AdditionalConstraint &constraint : mAdditionalConstraints)
-		constraint.mConstraint->mConstraintPriority = min(priorities[constraint.mBodyIdx[0]], priorities[constraint.mBodyIdx[1]]);
-}
-
-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].PreTranslated(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].PreTranslated(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;
-	BodyCreationSettings::MaterialToIDMap material_to_id;
-	BodyCreationSettings::GroupFilterToIDMap group_filter_to_id;
-
-	// Save skeleton
-	mSkeleton->SaveBinaryState(inStream);
-
-	// Save parts
-	inStream.Write((uint32)mParts.size());
-	for (const Part &p : mParts)
-	{
-		// Write body creation settings
-		p.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr);
-
-		// Save constraint
-		inStream.Write(p.mToParent != nullptr);
-		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)
-{
-	RagdollResult result;
-
-	// Restore skeleton
-	Skeleton::SkeletonResult skeleton_result = Skeleton::sRestoreFromBinaryState(inStream);
-	if (skeleton_result.HasError())
-	{
-		result.SetError(skeleton_result.GetError());
-		return result;
-	}
-
-	// Create ragdoll
-	Ref<RagdollSettings> ragdoll = new RagdollSettings();
-	ragdoll->mSkeleton = skeleton_result.Get();
-
-	BodyCreationSettings::IDToShapeMap id_to_shape;
-	BodyCreationSettings::IDToMaterialMap id_to_material;
-	BodyCreationSettings::IDToGroupFilterMap id_to_group_filter;
-
-	// Reserve some memory to avoid frequent reallocations
-	id_to_shape.reserve(1024);
-	id_to_material.reserve(128);
-	id_to_group_filter.reserve(128);
-
-	// Read parts
-	uint32 len = 0;
-	inStream.Read(len);
-	ragdoll->mParts.resize(len);
-	for (Part &p : ragdoll->mParts)
-	{
-		// Read creation settings
-		BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter);
-		if (bcs_result.HasError())
-		{
-			result.SetError(bcs_result.GetError());
-			return result;
-		}
-		static_cast<BodyCreationSettings &>(p) = bcs_result.Get();
-
-		// Read constraint
-		bool has_constraint = false;
-		inStream.Read(has_constraint);
-		if (has_constraint)
-		{
-			ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
-			if (constraint_result.HasError())
-			{
-				result.SetError(constraint_result.GetError());
-				return result;
-			}
-			p.mToParent = DynamicCast<TwoBodyConstraintSettings>(constraint_result.Get());
-		}
-	}
-
-	// 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());
-	}
-
-	// Create mapping tables
-	ragdoll->CalculateBodyIndexToConstraintIndex();
-	ragdoll->CalculateConstraintIndexToBodyIdxPair();
-
-	result.Set(ragdoll);
-	return result;
-}
-
-Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const
-{
-	Ragdoll *r = new Ragdoll(inSystem);
-	r->mRagdollSettings = this;
-	r->mBodyIDs.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;
-	for (const Part &p : mParts)
-	{
-		Body *body2 = bi.CreateBody(p);
-		if (body2 == nullptr)
-		{
-			// Out of bodies, failed to create ragdoll
-			delete r;
-			return nullptr;
-		}
-		body2->GetCollisionGroup().SetGroupID(inCollisionGroup);
-		body2->SetUserData(inUserData);
-
-		// Temporarily store body pointer for hooking up constraints
-		bodies[joint_idx] = body2;
-
-		// Create constraint
-		if (p.mToParent != nullptr)
-		{
-			Body *body1 = bodies[mSkeleton->GetJoint(joint_idx).mParentJointIndex];
-			r->mConstraints.push_back(p.mToParent->Create(*body1, *body2));
-		}
-
-		// Store body ID and constraint in parallel arrays
-		r->mBodyIDs.push_back(body2->GetID());
-
-		++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;
-}
-
-void RagdollSettings::CalculateBodyIndexToConstraintIndex()
-{
-	mBodyIndexToConstraintIndex.clear();
-	mBodyIndexToConstraintIndex.reserve(mParts.size());
-
-	int constraint_index = 0;
-	for (const Part &p : mParts)
-	{
-		if (p.mToParent != nullptr)
-			mBodyIndexToConstraintIndex.push_back(constraint_index++);
-		else
-			mBodyIndexToConstraintIndex.push_back(-1);
-	}
-}
-
-void RagdollSettings::CalculateConstraintIndexToBodyIdxPair()
-{
-	mConstraintIndexToBodyIdxPair.clear();
-	mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size());
-
-	// Add constraints between parts
-	int joint_idx = 0;
-	for (const Part &p : mParts)
-	{
-		if (p.mToParent != nullptr)
-		{
-			int parent_joint_idx = mSkeleton->GetJoint(joint_idx).mParentJointIndex;
-			mConstraintIndexToBodyIdxPair.emplace_back(parent_joint_idx, joint_idx);
-		}
-
-		++joint_idx;
-	}
-
-	// Add additional constraints
-	for (const AdditionalConstraint &c : mAdditionalConstraints)
-		mConstraintIndexToBodyIdxPair.emplace_back(c.mBodyIdx[0], c.mBodyIdx[1]);
-}
-
-Ragdoll::~Ragdoll()
-{
-	// Destroy all bodies
-	mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size());
-}
-
-void Ragdoll::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies)
-{
-	// Scope for JPH_STACK_ALLOC
-	{
-		// Create copy of body ids since they will be shuffled
-		int num_bodies = (int)mBodyIDs.size();
-		BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID));
-		memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID));
-
-		// Insert bodies as a batch
-		BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-		BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies, num_bodies);
-		bi.AddBodiesFinalize(bodies, num_bodies, add_state, inActivationMode);
-	}
-
-	// Add all constraints
-	mSystem->AddConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size());
-}
-
-void Ragdoll::RemoveFromPhysicsSystem(bool inLockBodies)
-{
-	// Remove all constraints before removing the bodies
-	mSystem->RemoveConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size());
-
-	// Scope for JPH_STACK_ALLOC
-	{
-		// Create copy of body ids since they will be shuffled
-		int num_bodies = (int)mBodyIDs.size();
-		BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID));
-		memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID));
-
-		// Remove all bodies as a batch
-		sRagdollGetBodyInterface(mSystem, inLockBodies).RemoveBodies(bodies, num_bodies);
-	}
-}
-
-void Ragdoll::Activate(bool inLockBodies)
-{
-	sRagdollGetBodyInterface(mSystem, inLockBodies).ActivateBodies(mBodyIDs.data(), (int)mBodyIDs.size());
-}
-
-bool Ragdoll::IsActive(bool inLockBodies) const
-{
-	// Lock the bodies
-	int body_count = (int)mBodyIDs.size();
-	BodyLockMultiRead lock(sRagdollGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
-
-	// Test if any body is active
-	for (int b = 0; b < body_count; ++b)
-	{
-		const Body *body = lock.GetBody(b);
-		if (body->IsActive())
-			return true;
-	}
-
-	return false;
-}
-
-void Ragdoll::SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies)
-{
-	// Lock the bodies
-	int body_count = (int)mBodyIDs.size();
-	BodyLockMultiWrite lock(sRagdollGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
-
-	// Update group ID
-	for (int b = 0; b < body_count; ++b)
-	{
-		Body *body = lock.GetBody(b);
-		body->GetCollisionGroup().SetGroupID(inGroupID);
-	}
-}
-
-void Ragdoll::SetPose(const SkeletonPose &inPose, bool inLockBodies)
-{
-	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
-
-	SetPose(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inLockBodies);
-}
-
-void Ragdoll::SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies)
-{
-	// Move bodies instantly into the correct position
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (int i = 0; i < (int)mBodyIDs.size(); ++i)
-	{
-		const Mat44 &joint = inJointMatrices[i];
-		bi.SetPositionAndRotation(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), EActivation::DontActivate);
-	}
-}
-
-void Ragdoll::GetPose(SkeletonPose &outPose, bool inLockBodies)
-{
-	JPH_ASSERT(outPose.GetSkeleton() == mRagdollSettings->mSkeleton);
-
-	RVec3 root_offset;
-	GetPose(root_offset, outPose.GetJointMatrices().data(), inLockBodies);
-	outPose.SetRootOffset(root_offset);
-}
-
-void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies)
-{
-	// Lock the bodies
-	int body_count = (int)mBodyIDs.size();
-	if (body_count == 0)
-		return;
-	BodyLockMultiRead lock(sRagdollGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
-
-	// Get root matrix
-	const Body *root = lock.GetBody(0);
-	RMat44 root_transform = root->GetWorldTransform();
-	outRootOffset = root_transform.GetTranslation();
-	outJointMatrices[0] = Mat44(root_transform.GetColumn4(0), root_transform.GetColumn4(1), root_transform.GetColumn4(2), Vec4(0, 0, 0, 1));
-
-	// Get other matrices
-	for (int b = 1; b < body_count; ++b)
-	{
-		const Body *body = lock.GetBody(b);
-		RMat44 transform = body->GetWorldTransform();
-		outJointMatrices[b] = Mat44(transform.GetColumn4(0), transform.GetColumn4(1), transform.GetColumn4(2), Vec4(Vec3(transform.GetTranslation() - outRootOffset), 1));
-	}
-}
-
-void Ragdoll::ResetWarmStart()
-{
-	for (TwoBodyConstraint *c : mConstraints)
-		c->ResetWarmStart();
-}
-
-void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies)
-{
-	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
-
-	DriveToPoseUsingKinematics(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inDeltaTime, inLockBodies);
-}
-
-void Ragdoll::DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies)
-{
-	// Move bodies into the correct position using kinematics
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (int i = 0; i < (int)mBodyIDs.size(); ++i)
-	{
-		const Mat44 &joint = inJointMatrices[i];
-		bi.MoveKinematic(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), inDeltaTime);
-	}
-}
-
-void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose)
-{
-	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
-
-	// Move bodies into the correct position using constraints
-	for (int i = 0; i < (int)inPose.GetJointMatrices().size(); ++i)
-	{
-		int constraint_idx = mRagdollSettings->GetConstraintIndexForBodyIndex(i);
-		if (constraint_idx >= 0)
-		{
-			// Get desired rotation of this body relative to its parent
-			const SkeletalAnimation::JointState &joint_state = inPose.GetJoint(i);
-
-			// Drive constraint to target
-			TwoBodyConstraint *constraint = mConstraints[constraint_idx];
-			EConstraintSubType sub_type = constraint->GetSubType();
-			if (sub_type == EConstraintSubType::SwingTwist)
-			{
-				SwingTwistConstraint *st_constraint = static_cast<SwingTwistConstraint *>(constraint);
-				st_constraint->SetSwingMotorState(EMotorState::Position);
-				st_constraint->SetTwistMotorState(EMotorState::Position);
-				st_constraint->SetTargetOrientationBS(joint_state.mRotation);
-			}
-			else if (sub_type == EConstraintSubType::Hinge)
-			{
-				HingeConstraint *h_constraint = static_cast<HingeConstraint *>(constraint);
-				h_constraint->SetMotorState(EMotorState::Position);
-				h_constraint->SetTargetOrientationBS(joint_state.mRotation);
-			}
-			else
-				JPH_ASSERT(false, "Constraint type not implemented!");
-		}
-	}
-}
-
-void Ragdoll::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies)
-{
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (BodyID body_id : mBodyIDs)
-		bi.SetLinearAndAngularVelocity(body_id, inLinearVelocity, inAngularVelocity);
-}
-
-void Ragdoll::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
-{
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (BodyID body_id : mBodyIDs)
-		bi.SetLinearVelocity(body_id, inLinearVelocity);
-}
-
-void Ragdoll::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
-{
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (BodyID body_id : mBodyIDs)
-		bi.AddLinearVelocity(body_id, inLinearVelocity);
-}
-
-void Ragdoll::AddImpulse(Vec3Arg inImpulse, bool inLockBodies)
-{
-	BodyInterface &bi = sRagdollGetBodyInterface(mSystem, inLockBodies);
-	for (BodyID body_id : mBodyIDs)
-		bi.AddImpulse(body_id, inImpulse);
-}
-
-void Ragdoll::GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const
-{
-	BodyLockRead lock(sRagdollGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs[0]);
-	if (lock.Succeeded())
-	{
-		const Body &body = lock.GetBody();
-		outPosition = body.GetPosition();
-		outRotation = body.GetRotation();
-	}
-	else
-	{
-		outPosition = RVec3::sZero();
-		outRotation = Quat::sIdentity();
-	}
-}
-
-AABox Ragdoll::GetWorldSpaceBounds(bool inLockBodies) const
-{
-	// Lock the bodies
-	int body_count = (int)mBodyIDs.size();
-	BodyLockMultiRead lock(sRagdollGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
-
-	// Encapsulate all bodies
-	AABox bounds;
-	for (int b = 0; b < body_count; ++b)
-	{
-		const Body *body = lock.GetBody(b);
-		if (body != nullptr)
-			bounds.Encapsulate(body->GetWorldSpaceBounds());
-	}
-	return bounds;
-}
-
-JPH_NAMESPACE_END