From 23d752e07d1ead3168261d4b21b1c10058cf1178 Mon Sep 17 00:00:00 2001 From: greerdv Date: Tue, 16 Nov 2021 12:50:11 +0000 Subject: [PATCH 1/2] add option to clamp ragdoll joint masses to avoid bad mass ratios Signed-off-by: greerdv --- .../PhysXCharacters/API/CharacterUtils.cpp | 49 +++++++ .../PhysXCharacters/API/CharacterUtils.h | 12 ++ .../Components/RagdollComponent.cpp | 125 +++++++++++++----- .../Components/RagdollComponent.h | 5 + Gems/PhysX/Code/Tests/RagdollTests.cpp | 15 +++ 5 files changed, 174 insertions(+), 32 deletions(-) diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp index 4ed2825cc2..05de1055b2 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp +++ b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp @@ -344,6 +344,55 @@ namespace PhysX bool isAcceleration = true; return physx::PxD6JointDrive(stiffness, damping, forceLimit, isAcceleration); } + + AZStd::vector ComputeHierarchyDepths(const AZStd::vector& parentIndices) + { + const size_t numNodes = parentIndices.size(); + AZStd::vector nodeDepths(numNodes); + for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++) + { + nodeDepths[nodeIndex] = { -1, nodeIndex }; + } + + for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++) + { + if (nodeDepths[nodeIndex].m_depth != -1) + { + continue; + } + int depth = -1; // initial depth value for this node + int ancestorDepth = 0; // the depth of the first ancestor we find when iteratively visiting parents + bool ancestorFound = false; // whether we have found either an ancestor which already has a depth value, or the root + size_t currentIndex = nodeIndex; + while (!ancestorFound) + { + depth++; + if (depth > numNodes) + { + AZ_Assert(false, "Loop detected in ragdoll configuration."); + return nodeDepths; + } + size_t parentIndex = parentIndices[currentIndex]; + + if (parentIndex >= numNodes || nodeDepths[currentIndex].m_depth != -1) + { + ancestorFound = true; + ancestorDepth = (nodeDepths[currentIndex].m_depth != -1) ? nodeDepths[currentIndex].m_depth : 0; + } + + currentIndex = parentIndex; + } + + currentIndex = nodeIndex; + for (int i = depth; i >= 0 && currentIndex < numNodes; i--) + { + nodeDepths[currentIndex] = { ancestorDepth + i, currentIndex }; + currentIndex = parentIndices[currentIndex]; + } + } + + return nodeDepths; + } } // namespace Characters } // namespace Utils } // namespace PhysX diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.h b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.h index 0f51a5d9b9..245dc01abd 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.h +++ b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.h @@ -49,6 +49,18 @@ namespace PhysX //! @param forceLimit The upper limit on the force the joint can apply to reach its target. //! @return The created joint drive. physx::PxD6JointDrive CreateD6JointDrive(float strength, float dampingRatio, float forceLimit); + + //! Contains information about a node in a hierarchy and how deep it is in the hierarchy relative to the root. + struct DepthData + { + int m_depth = -1; //!< Depth of the joint in the hierarchy. The root has depth 0, its children depth 1, and so on. + size_t m_index = 0; // ComputeHierarchyDepths(const AZStd::vector& parentIndices); } // namespace Characters } // namespace Utils } // namespace PhysX diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp index 57972fea3e..f1b3db69e6 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp +++ b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp @@ -7,20 +7,20 @@ */ #include -#include #include -#include #include +#include +#include #include +#include #include #include -#include +#include #include namespace PhysX { - bool RagdollComponent::VersionConverter(AZ::SerializeContext& context, - AZ::SerializeContext::DataElementNode& classElement) + bool RagdollComponent::VersionConverter(AZ::SerializeContext& context, AZ::SerializeContext::DataElementNode& classElement) { // The element "PhysXRagdoll" was changed from a shared pointer to a unique pointer, but a version converter was // not added at the time. This means there may be serialized data with either the shared or unique pointer, but @@ -76,13 +76,13 @@ namespace PhysX ->Field("EnableJointProjection", &RagdollComponent::m_enableJointProjection) ->Field("ProjectionLinearTol", &RagdollComponent::m_jointProjectionLinearTolerance) ->Field("ProjectionAngularTol", &RagdollComponent::m_jointProjectionAngularToleranceDegrees) - ; + ->Field("EnableMassRatioClamping", &RagdollComponent::m_enableMassRatioClamping) + ->Field("MaxMassRatio", &RagdollComponent::m_maxMassRatio); AZ::EditContext* editContext = serializeContext->GetEditContext(); if (editContext) { - editContext->Class( - "PhysX Ragdoll", "Creates a PhysX ragdoll simulation for an animation actor.") + editContext->Class("PhysX Ragdoll", "Creates a PhysX ragdoll simulation for an animation actor.") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "PhysX") ->Attribute(AZ::Edit::Attributes::Icon, "Icons/Components/PhysXRagdoll.svg") @@ -90,34 +90,49 @@ namespace PhysX ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game", 0x232b318c)) ->Attribute(AZ::Edit::Attributes::HelpPageURL, "https://o3de.org/docs/user-guide/components/reference/physx/ragdoll/") ->Attribute(AZ::Edit::Attributes::AutoExpand, true) - ->DataElement(AZ::Edit::UIHandlers::Default, &RagdollComponent::m_positionIterations, "Position Iteration Count", + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_positionIterations, "Position Iteration Count", "The frequency at which ragdoll collider positions are resolved. Higher values can increase fidelity but decrease " "performance. Very high values might introduce instability.") ->Attribute(AZ::Edit::Attributes::Min, 1) ->Attribute(AZ::Edit::Attributes::Max, 255) - ->DataElement(AZ::Edit::UIHandlers::Default, &RagdollComponent::m_velocityIterations, "Velocity Iteration Count", + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_velocityIterations, "Velocity Iteration Count", "The frequency at which ragdoll collider velocities are resolved. Higher values can increase fidelity but decrease " "performance. Very high values might introduce instability.") ->Attribute(AZ::Edit::Attributes::Min, 1) ->Attribute(AZ::Edit::Attributes::Max, 255) - ->DataElement(AZ::Edit::UIHandlers::Default, &RagdollComponent::m_enableJointProjection, - "Enable Joint Projection", "When active, preserves joint constraints in volatile simulations. " + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_enableJointProjection, "Enable Joint Projection", + "When active, preserves joint constraints in volatile simulations. " "Might not be physically correct in all simulations.") ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree) - ->DataElement(AZ::Edit::UIHandlers::Default, &RagdollComponent::m_jointProjectionLinearTolerance, + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_jointProjectionLinearTolerance, "Joint Projection Linear Tolerance", "Maximum linear joint error. Projection is applied to linear joint errors above this value.") ->Attribute(AZ::Edit::Attributes::Min, 0.0f) ->Attribute(AZ::Edit::Attributes::Step, 1e-3f) ->Attribute(AZ::Edit::Attributes::Visibility, &RagdollComponent::IsJointProjectionVisible) - ->DataElement(AZ::Edit::UIHandlers::Default, &RagdollComponent::m_jointProjectionAngularToleranceDegrees, + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_jointProjectionAngularToleranceDegrees, "Joint Projection Angular Tolerance", "Maximum angular joint error. Projection is applied to angular joint errors above this value.") ->Attribute(AZ::Edit::Attributes::Min, 0.0f) ->Attribute(AZ::Edit::Attributes::Step, 0.1f) ->Attribute(AZ::Edit::Attributes::Suffix, " degrees") ->Attribute(AZ::Edit::Attributes::Visibility, &RagdollComponent::IsJointProjectionVisible) - ; + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_enableMassRatioClamping, "Enable Mass Ratio Clamping", + "When active, ragdoll node mass values may be overridden to avoid unstable mass ratios.") + ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree) + ->DataElement( + AZ::Edit::UIHandlers::Default, &RagdollComponent::m_maxMassRatio, "Maximum Mass Ratio", + "The mass of the child body of a joint may be clamped to avoid its ratio with the parent " + "body mass exceeding this threshold.") + ->Attribute(AZ::Edit::Attributes::Min, 1.0f) + ->Attribute(AZ::Edit::Attributes::Step, 0.1f) + ->Attribute(AZ::Edit::Attributes::Visibility, &RagdollComponent::IsMaxMassRatioVisible); } } @@ -131,6 +146,11 @@ namespace PhysX return m_enableJointProjection; } + bool RagdollComponent::IsMaxMassRatioVisible() + { + return m_enableMassRatioClamping; + } + // AZ::Component void RagdollComponent::Init() { @@ -272,7 +292,6 @@ namespace PhysX return ragdoll->IsSimulated(); } return false; - } AZ::Aabb RagdollComponent::GetAabb() const @@ -318,20 +337,19 @@ namespace PhysX if (numNodes == 0) { - AZ_Error("PhysX Ragdoll Component", false, - "Ragdoll configuration has 0 nodes, ragdoll will not be created for entity \"%s\".", + AZ_Error( + "PhysX Ragdoll Component", false, "Ragdoll configuration has 0 nodes, ragdoll will not be created for entity \"%s\".", GetEntity()->GetName().c_str()); return; } - ragdollConfiguration.m_parentIndices.resize(numNodes); for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++) { AZStd::string parentName; AZStd::string nodeName = ragdollConfiguration.m_nodes[nodeIndex].m_debugName; - AzFramework::CharacterPhysicsDataRequestBus::EventResult(parentName, GetEntityId(), - &AzFramework::CharacterPhysicsDataRequests::GetParentNodeName, nodeName); + AzFramework::CharacterPhysicsDataRequestBus::EventResult( + parentName, GetEntityId(), &AzFramework::CharacterPhysicsDataRequests::GetParentNodeName, nodeName); AZ::Outcome parentIndex = Utils::Characters::GetNodeIndex(ragdollConfiguration, parentName); ragdollConfiguration.m_parentIndices[nodeIndex] = parentIndex ? parentIndex.GetValue() : SIZE_MAX; @@ -339,8 +357,8 @@ namespace PhysX } Physics::RagdollState bindPose; - AzFramework::CharacterPhysicsDataRequestBus::EventResult(bindPose, GetEntityId(), - &AzFramework::CharacterPhysicsDataRequests::GetBindPose, ragdollConfiguration); + AzFramework::CharacterPhysicsDataRequestBus::EventResult( + bindPose, GetEntityId(), &AzFramework::CharacterPhysicsDataRequests::GetBindPose, ragdollConfiguration); AZ::Transform entityTransform = AZ::Transform::CreateIdentity(); AZ::TransformBus::EventResult(entityTransform, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM); @@ -354,13 +372,12 @@ namespace PhysX m_ragdollHandle = sceneInterface->AddSimulatedBody(m_attachedSceneHandle, &ragdollConfiguration); } auto* ragdoll = GetPhysXRagdoll(); - if (ragdoll == nullptr || - m_ragdollHandle == AzPhysics::InvalidSimulatedBodyHandle) + if (ragdoll == nullptr || m_ragdollHandle == AzPhysics::InvalidSimulatedBodyHandle) { AZ_Error("PhysX Ragdoll Component", false, "Failed to create ragdoll."); return; } - + for (size_t nodeIndex = 0; nodeIndex < numNodes; nodeIndex++) { if (physx::PxRigidDynamic* pxRigidBody = ragdoll->GetPxRigidDynamic(nodeIndex)) @@ -389,17 +406,62 @@ namespace PhysX } } + // If mass ratio clamping is enabled, iterate out from the root and clamp mass values + if (m_enableMassRatioClamping) + { + const float maxMassRatio = AZStd::GetMax(1.0f + AZ::Constants::FloatEpsilon, m_maxMassRatio); + + // figure out the depth of each node in the tree, so that nodes can be visited from the root outwards + AZStd::vector nodeDepths = + Utils::Characters::ComputeHierarchyDepths(ragdollConfiguration.m_parentIndices); + + AZStd::sort( + nodeDepths.begin(), nodeDepths.end(), + [](const Utils::Characters::DepthData& d1, const Utils::Characters::DepthData& d2) + { + return d1.m_depth < d2.m_depth; + }); + + bool massesClamped = false; + for (const auto [depth, nodeIndex] : nodeDepths) + { + size_t parentIndex = ragdollConfiguration.m_parentIndices[nodeIndex]; + if (parentIndex < numNodes) + { + AzPhysics::RigidBody& nodeRigidBody = ragdoll->GetNode(nodeIndex)->GetRigidBody(); + const float originalMass = nodeRigidBody.GetMass(); + const float parentMass = ragdoll->GetNode(parentIndex)->GetRigidBody().GetMass(); + const float minMass = parentMass / maxMassRatio; + const float maxMass = parentMass; + if (originalMass < minMass || originalMass > maxMass) + { + const float clampedMass = AZStd::clamp(originalMass, minMass, maxMass); + nodeRigidBody.SetMass(clampedMass); + massesClamped = true; + if (!AZ::IsCloseMag(originalMass, 0.0f)) + { + // scale the inertia proportionally to how the mass was modified + auto pxRigidBody = static_cast(nodeRigidBody.GetNativePointer()); + pxRigidBody->setMassSpaceInertiaTensor(clampedMass / originalMass * pxRigidBody->getMassSpaceInertiaTensor()); + } + } + } + } + + AZ_WarningOnce("PhysX Ragdoll", !massesClamped, + "Mass values for ragdoll on entity \"%s\" were modified based on max mass ratio setting to avoid instability.", + GetEntity()->GetName().c_str()); + } + AzFramework::RagdollPhysicsRequestBus::Handler::BusConnect(GetEntityId()); AzPhysics::SimulatedBodyComponentRequestsBus::Handler::BusConnect(GetEntityId()); - AzFramework::RagdollPhysicsNotificationBus::Event(GetEntityId(), - &AzFramework::RagdollPhysicsNotifications::OnRagdollActivated); + AzFramework::RagdollPhysicsNotificationBus::Event(GetEntityId(), &AzFramework::RagdollPhysicsNotifications::OnRagdollActivated); } void RagdollComponent::DestroyRagdoll() { - if (m_ragdollHandle != AzPhysics::InvalidSimulatedBodyHandle && - m_attachedSceneHandle != AzPhysics::InvalidSceneHandle) + if (m_ragdollHandle != AzPhysics::InvalidSimulatedBodyHandle && m_attachedSceneHandle != AzPhysics::InvalidSceneHandle) { AzFramework::RagdollPhysicsRequestBus::Handler::BusDisconnect(); AzFramework::RagdollPhysicsNotificationBus::Event( @@ -421,8 +483,7 @@ namespace PhysX const Ragdoll* RagdollComponent::GetPhysXRagdollConst() const { - if (m_ragdollHandle == AzPhysics::InvalidSimulatedBodyHandle || - m_attachedSceneHandle == AzPhysics::InvalidSceneHandle) + if (m_ragdollHandle == AzPhysics::InvalidSimulatedBodyHandle || m_attachedSceneHandle == AzPhysics::InvalidSceneHandle) { return nullptr; } diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h index 63dd1354dd..1eb46d183d 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h +++ b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h @@ -104,6 +104,7 @@ namespace PhysX const Ragdoll* GetPhysXRagdollConst() const; bool IsJointProjectionVisible(); + bool IsMaxMassRatioVisible(); AzPhysics::SimulatedBodyHandle m_ragdollHandle = AzPhysics::InvalidSimulatedBodyHandle; AzPhysics::SceneHandle m_attachedSceneHandle = AzPhysics::InvalidSceneHandle; @@ -119,5 +120,9 @@ namespace PhysX float m_jointProjectionLinearTolerance = 1e-3f; /// Angular joint error (in degrees) above which projection will be applied. float m_jointProjectionAngularToleranceDegrees = 1.0f; + /// Allows ragdoll node mass values to be overridden to avoid unstable mass ratios. + bool m_enableMassRatioClamping = false; + /// If mass ratio clamping is enabled, masses will be clamped to within this ratio. + float m_maxMassRatio = 2.0f; }; } // namespace PhysX diff --git a/Gems/PhysX/Code/Tests/RagdollTests.cpp b/Gems/PhysX/Code/Tests/RagdollTests.cpp index 30e9400790..ff4637fd6e 100644 --- a/Gems/PhysX/Code/Tests/RagdollTests.cpp +++ b/Gems/PhysX/Code/Tests/RagdollTests.cpp @@ -367,4 +367,19 @@ namespace PhysX float minZ = ragdoll->GetAabb().GetMin().GetZ(); EXPECT_NEAR(minZ, 0.0f, 0.05f); } + + TEST(ComputeHierarchyDepthsTest, DepthValuesCorrect) + { + AZStd::vector parentIndices = + { 3, 5, AZStd::numeric_limits::max(), 1, 2, 9, 7, 4, 0, 6, 11, 12, 5, 14, 15, 16, 5, 18, 19, 4, 21, 22, 4 }; + + const AZStd::vector nodeDepths = Utils::Characters::ComputeHierarchyDepths(parentIndices); + + std::vector expectedDepths = { 8, 6, 0, 7, 1, 5, 3, 2, 9, 4, 8, 7, 6, 9, 8, 7, 6, 4, 3, 2, 4, 3, 2 }; + + for (size_t i = 0; i < parentIndices.size(); i++) + { + EXPECT_EQ(nodeDepths[i].m_depth, expectedDepths[i]); + } + } } // namespace PhysX From 5594a61ece5f502b9f49e425529c1b79f5f0b5d0 Mon Sep 17 00:00:00 2001 From: greerdv Date: Tue, 16 Nov 2021 15:56:56 +0000 Subject: [PATCH 2/2] apply feedback from PR Signed-off-by: greerdv --- .../Source/PhysXCharacters/API/CharacterUtils.cpp | 6 +++--- .../PhysXCharacters/Components/RagdollComponent.cpp | 11 ++++++----- .../PhysXCharacters/Components/RagdollComponent.h | 4 ++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp index 05de1055b2..46f4c04880 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp +++ b/Gems/PhysX/Code/Source/PhysXCharacters/API/CharacterUtils.cpp @@ -369,10 +369,10 @@ namespace PhysX depth++; if (depth > numNodes) { - AZ_Assert(false, "Loop detected in ragdoll configuration."); + AZ_Error("PhysX Ragdoll", false, "Loop detected in hierarchy depth computation."); return nodeDepths; } - size_t parentIndex = parentIndices[currentIndex]; + const size_t parentIndex = parentIndices[currentIndex]; if (parentIndex >= numNodes || nodeDepths[currentIndex].m_depth != -1) { @@ -384,7 +384,7 @@ namespace PhysX } currentIndex = nodeIndex; - for (int i = depth; i >= 0 && currentIndex < numNodes; i--) + for (int i = depth; i >= 0; i--) { nodeDepths[currentIndex] = { ancestorDepth + i, currentIndex }; currentIndex = parentIndices[currentIndex]; diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp index f1b3db69e6..7615a175d1 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp +++ b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.cpp @@ -141,12 +141,12 @@ namespace PhysX } } - bool RagdollComponent::IsJointProjectionVisible() + bool RagdollComponent::IsJointProjectionVisible() const { return m_enableJointProjection; } - bool RagdollComponent::IsMaxMassRatioVisible() + bool RagdollComponent::IsMaxMassRatioVisible() const { return m_enableMassRatioClamping; } @@ -423,9 +423,10 @@ namespace PhysX }); bool massesClamped = false; - for (const auto [depth, nodeIndex] : nodeDepths) + for (const auto& nodeDepth : nodeDepths) { - size_t parentIndex = ragdollConfiguration.m_parentIndices[nodeIndex]; + const size_t nodeIndex = nodeDepth.m_index; + const size_t parentIndex = ragdollConfiguration.m_parentIndices[nodeIndex]; if (parentIndex < numNodes) { AzPhysics::RigidBody& nodeRigidBody = ragdoll->GetNode(nodeIndex)->GetRigidBody(); @@ -438,7 +439,7 @@ namespace PhysX const float clampedMass = AZStd::clamp(originalMass, minMass, maxMass); nodeRigidBody.SetMass(clampedMass); massesClamped = true; - if (!AZ::IsCloseMag(originalMass, 0.0f)) + if (!AZ::IsClose(originalMass, 0.0f)) { // scale the inertia proportionally to how the mass was modified auto pxRigidBody = static_cast(nodeRigidBody.GetNativePointer()); diff --git a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h index 1eb46d183d..3ef59bf623 100644 --- a/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h +++ b/Gems/PhysX/Code/Source/PhysXCharacters/Components/RagdollComponent.h @@ -103,8 +103,8 @@ namespace PhysX Ragdoll* GetPhysXRagdoll(); const Ragdoll* GetPhysXRagdollConst() const; - bool IsJointProjectionVisible(); - bool IsMaxMassRatioVisible(); + bool IsJointProjectionVisible() const; + bool IsMaxMassRatioVisible() const; AzPhysics::SimulatedBodyHandle m_ragdollHandle = AzPhysics::InvalidSimulatedBodyHandle; AzPhysics::SceneHandle m_attachedSceneHandle = AzPhysics::InvalidSceneHandle;