initial commit, 4.5 stable
Some checks failed
🔗 GHA / 📊 Static checks (push) Has been cancelled
🔗 GHA / 🤖 Android (push) Has been cancelled
🔗 GHA / 🍏 iOS (push) Has been cancelled
🔗 GHA / 🐧 Linux (push) Has been cancelled
🔗 GHA / 🍎 macOS (push) Has been cancelled
🔗 GHA / 🏁 Windows (push) Has been cancelled
🔗 GHA / 🌐 Web (push) Has been cancelled

This commit is contained in:
2025-09-16 20:46:46 -04:00
commit 9d30169a8d
13378 changed files with 7050105 additions and 0 deletions

View File

@@ -0,0 +1,705 @@
// 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/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::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
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

View File

@@ -0,0 +1,240 @@
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#pragma once
#include <Jolt/Core/Reference.h>
#include <Jolt/Core/Result.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
#include <Jolt/Skeleton/Skeleton.h>
#include <Jolt/Skeleton/SkeletonPose.h>
#include <Jolt/Physics/EActivation.h>
JPH_NAMESPACE_BEGIN
class Ragdoll;
class PhysicsSystem;
/// Contains the structure of a ragdoll
class JPH_EXPORT RagdollSettings : public RefTarget<RagdollSettings>
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RagdollSettings)
public:
/// Stabilize the constraints of the ragdoll
/// @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)
/// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared)
void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const;
using RagdollResult = Result<Ref<RagdollSettings>>;
/// Restore a saved ragdoll from inStream
static RagdollResult sRestoreFromBinaryState(StreamIn &inStream);
/// Create ragdoll instance from these settings
/// @return Newly created ragdoll or null when out of bodies
Ragdoll * CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const;
/// Access to the skeleton of this ragdoll
const Skeleton * GetSkeleton() const { return mSkeleton; }
Skeleton * GetSkeleton() { return mSkeleton; }
/// Calculate the map needed for GetBodyIndexToConstraintIndex()
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
int GetConstraintIndexForBodyIndex(int inBodyIndex) const { return mBodyIndexToConstraintIndex[inBodyIndex]; }
/// Calculate the map needed for GetConstraintIndexToBodyIdxPair()
void CalculateConstraintIndexToBodyIdxPair();
using BodyIdxPair = std::pair<int, int>;
/// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
const Array<BodyIdxPair> & GetConstraintIndexToBodyIdxPair() const { return mConstraintIndexToBodyIdxPair; }
/// Map a single constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
BodyIdxPair GetBodyIndicesForConstraintIndex(int inConstraintIndex) const { return mConstraintIndexToBodyIdxPair[inConstraintIndex]; }
/// A single rigid body sub part of the ragdoll
class Part : public BodyCreationSettings
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Part)
public:
Ref<TwoBodyConstraintSettings> mToParent;
};
/// 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
{
JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint)
public:
/// 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;
/// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
Array<BodyIdxPair> mConstraintIndexToBodyIdxPair;
};
/// Runtime ragdoll information
class JPH_EXPORT Ragdoll : public RefTarget<Ragdoll>, public NonCopyable
{
public:
JPH_OVERRIDE_NEW_DELETE
/// Constructor
explicit Ragdoll(PhysicsSystem *inSystem) : mSystem(inSystem) { }
/// Destructor
~Ragdoll();
/// Add bodies and constraints to the system and optionally activate the bodies
void AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies = true);
/// Remove bodies and constraints from the system
void RemoveFromPhysicsSystem(bool inLockBodies = true);
/// Wake up all bodies in the ragdoll
void Activate(bool inLockBodies = true);
/// Check if one or more of the bodies in the ragdoll are active.
/// Note that this involves locking the bodies (if inLockBodies is true) and looping over them. An alternative and possibly faster
/// way could be to install a BodyActivationListener and count the number of active bodies of a ragdoll as they're activated / deactivated
/// (basically check if the body that activates / deactivates is in GetBodyIDs() and increment / decrement a counter).
bool IsActive(bool inLockBodies = true) const;
/// 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 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);
/// Get the ragdoll pose (uses the world transform of the bodies to calculate the pose)
void GetPose(SkeletonPose &outPose, bool inLockBodies = true);
/// Lower level version of GetPose that directly returns the world space joint matrices
void GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true);
/// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart.
void ResetWarmStart();
/// 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);
/// Drive the ragdoll to a specific pose by activating the motors on each constraint
void DriveToPoseUsingMotors(const SkeletonPose &inPose);
/// 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);
/// Add impulse to all bodies of the ragdoll (center of mass of each of them)
void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true);
/// Get the position and orientation of the root of the ragdoll
void GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
/// Get number of bodies in the ragdoll
size_t GetBodyCount() const { return mBodyIDs.size(); }
/// Access a body ID
BodyID GetBodyID(int inBodyIndex) const { return mBodyIDs[inBodyIndex]; }
/// Access to the array of body IDs
const Array<BodyID> & GetBodyIDs() const { return mBodyIDs; }
/// Get number of constraints in the ragdoll
size_t GetConstraintCount() const { return mConstraints.size(); }
/// Access a constraint by index
TwoBodyConstraint * GetConstraint(int inConstraintIndex) { return mConstraints[inConstraintIndex]; }
/// Access a constraint by index
const TwoBodyConstraint * GetConstraint(int inConstraintIndex) const { return mConstraints[inConstraintIndex]; }
/// Get world space bounding box for all bodies of the ragdoll
AABox GetWorldSpaceBounds(bool inLockBodies = true) const;
/// Get the settings object that created this ragdoll
const RagdollSettings * GetRagdollSettings() const { return mRagdollSettings; }
private:
/// For RagdollSettings::CreateRagdoll function
friend class RagdollSettings;
/// The settings that created this ragdoll
RefConst<RagdollSettings> mRagdollSettings;
/// The bodies and constraints that this ragdoll consists of (1-on-1 with mRagdollSettings->mParts)
Array<BodyID> mBodyIDs;
/// Array of constraints that connect the bodies together
Array<Ref<TwoBodyConstraint>> mConstraints;
/// Cached physics system
PhysicsSystem * mSystem;
};
JPH_NAMESPACE_END