// Copyright Epic Games, Inc. All Rights Reserved. #include "RigPhysicsSimulation.h" #include "RigPhysicsBodyComponent.h" #include "RigPhysicsJointComponent.h" #include "RigPhysicsControlComponent.h" #include "RigPhysicsSolverComponent.h" #include "AnimNode_RigidBodyWithControl.h" #include "PhysicsControlPoseData.h" #include "PhysicsControlHelpers.h" #include "ControlRig.h" #include "Rigs/RigHierarchy.h" #include "Rigs/RigHierarchyController.h" #include "Units/RigUnitContext.h" #include "RigVMCore/RigVMExecuteContext.h" #include "Engine/World.h" #include "Physics/ImmediatePhysics/ImmediatePhysicsActorHandle.h" #include "Physics/ImmediatePhysics/ImmediatePhysicsAdapters.h" #include "Physics/ImmediatePhysics/ImmediatePhysicsJointHandle.h" #include "Physics/ImmediatePhysics/ImmediatePhysicsSimulation.h" #include "Physics/ImmediatePhysics/ImmediatePhysicsChaos/ImmediatePhysicsSimulation_Chaos.h" #include "Physics/Experimental/PhysScene_Chaos.h" #include "Physics/Experimental/ChaosInterfaceUtils.h" #include "Chaos/ParticleHandle.h" #include "Chaos/PBDJointConstraints.h" #include "Chaos/ImplicitObject.h" #include "Chaos/ShapeInstance.h" #include "Chaos/Capsule.h" #include "Chaos/ChaosScene.h" #include "Chaos/PBDJointConstraintUtilities.h" #include "Chaos/Evolution/SimulationSpace.h" #include "Chaos/PBDCollisionConstraints.h" #include "PhysicsEngine/BodyInstance.h" #include "PhysicsEngine/ConstraintInstance.h" #include "PhysicsEngine/BodySetup.h" #if WITH_CHAOS_VISUAL_DEBUGGER #include "ChaosVDRuntimeModule.h" #endif #include "Logging/LogMacros.h" DEFINE_LOG_CATEGORY(LogRigPhysics); TAutoConsoleVariable CVarControlRigPhysicsFixedTimeStepOverride( TEXT("ControlRig.Physics.FixedTimeStepOverride"), -1.0f, TEXT("-1.0 disables the override, so the timestep authored in the simulation settings will be used (which may or may not imply a fixed timestep). A value of 0 forces a variable timestep to be used. A +ve value is used to specify a fixed timestep.")); TAutoConsoleVariable CVarControlRigPhysicsMaxTimeStepsOverride( TEXT("ControlRig.Physics.MaxTimeStepsOverride"), -1, TEXT("-1 disables the override, so the max timesteps authored in the simulation settings will be used. A +ve value is used to specify the maximum number of timesteps.")); TAutoConsoleVariable CVarControlRigPhysicsMaxDeltaTimeOverride( TEXT("ControlRig.Physics.MaxDeltaTimeOverride"), -1, TEXT("-1 disables the override, so the max delta time authored in the simulation settings will be used. A +ve value is used to specify the maximum delta time.")); constexpr int32 ConstraintChildIndex = 0; constexpr int32 ConstraintParentIndex = 1; //====================================================================================================================== FRigPhysicsSimulation::FRigPhysicsSimulation(TObjectPtr InOwningControlRig) : FRigPhysicsSimulationBase(FRigPhysicsSimulation::StaticStruct()), OwningControlRig(InOwningControlRig) { } //====================================================================================================================== bool FRigPhysicsSimulation::ShouldComponentBeInSimulation( FRigComponentKey SolverComponentKey, FRigComponentKey ComponentKey) const { const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey)); const FRigPhysicsSolverComponent* PhysicsSolverComponent = Cast( Hierarchy->FindComponent(SolverComponentKey)); if (!PhysicsComponent || !PhysicsSolverComponent) { return false; } if (PhysicsComponent->BodySolverSettings.PhysicsSolverComponentKey == SolverComponentKey) { return true; } if (!PhysicsSolverComponent->SolverSettings.bAutomaticallyAddPhysicsComponents) { return false; } if (PhysicsComponent->BodySolverSettings.bUseAutomaticSolver) { FRigElementKey ElementKey = PhysicsComponent->GetElementKey(); while (ElementKey.IsValid()) { for (FRigComponentKey CK : Hierarchy->GetComponentKeys(ElementKey)) { if (CK == SolverComponentKey) { return true; } } // Note that getting the parent of an element at the root doesn't return the top-level element ElementKey = Hierarchy->GetFirstParent(ElementKey); } } return false; } //====================================================================================================================== void FRigPhysicsSimulation::InitialiseSimulation(const FRigPhysicsSolverComponent* SolverComponent) { DestroyPhysicsSimulation(); Simulation = MakeShared(); #if CHAOS_DEBUG_NAME FString SimName = TEXT("ControlRigPhysics-"); SimName += OwningControlRig->GetName(); Simulation->SetDebugName(FName(SimName)); #endif #if WITH_CHAOS_VISUAL_DEBUGGER Simulation->GetChaosVDContextData().Id = FChaosVDRuntimeModule::Get().GenerateUniqueID(); Simulation->GetChaosVDContextData().Type = static_cast(EChaosVDContextType::Solver); #endif // This is needed so that when using a fixed timestep, velocities are rewound as well as // positions. This is not only more accurate, but it's needed in order to get soft constraint // behavior (in particular, for controls) that behave fairly independently of the control-rig // tick rate. Simulation->SetRewindVelocities(true); // Always create a world actor at the origin, for attaching controls to. SimulationActorHandle = CreateBody( FName(TEXT("Simulation")), FRigPhysicsCollision(), nullptr, nullptr, FTransform()); } //====================================================================================================================== void FRigPhysicsSimulation::InitialiseControlRecords(const FRigPhysicsSolverComponent* SolverComponent) { ensure(ControlRecords.IsEmpty()); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); if (!Hierarchy || !SolverComponent) { return; } FRigComponentKey SolverComponentKey = SolverComponent->GetKey(); TArray AllComponentKeys = Hierarchy->GetAllComponentKeys(); for (FRigComponentKey ComponentKey : AllComponentKeys) { if (const FRigPhysicsControlComponent* ControlComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { // The authored body components may be blank, in which case we need to find the automatic ones. FRigControlRecord ControlRecord; ControlRecord.ParentBodyComponentKey = ControlComponent->ParentBodyComponentKey; ControlRecord.ChildBodyComponentKey = ControlComponent->ChildBodyComponentKey; // Automate the child if (!ControlRecord.ChildBodyComponentKey.IsValid()) { TArray SiblingComponentKeys = Hierarchy->GetComponentKeys(ComponentKey.ElementKey); for (FRigComponentKey SiblingComponentKey : SiblingComponentKeys) { if (ShouldComponentBeInSimulation(SolverComponentKey, SiblingComponentKey)) { ControlRecord.ChildBodyComponentKey = SiblingComponentKey; break; } } } if (ControlComponent->bUseParentBodyAsDefault) { if (!ControlRecord.ParentBodyComponentKey.IsValid()) { FRigElementKey ParentElementKey = Hierarchy->GetFirstParent(ComponentKey.ElementKey); TArray ParentComponentKeys = Hierarchy->GetComponentKeys(ParentElementKey); for (FRigComponentKey ParentComponentKey : ParentComponentKeys) { if (ShouldComponentBeInSimulation(SolverComponentKey, ParentComponentKey)) { ControlRecord.ParentBodyComponentKey = ParentComponentKey; break; } } } } if (ShouldComponentBeInSimulation(SolverComponentKey, ControlRecord.ChildBodyComponentKey)) { // Here, an invalid parent component key indicates a sim-space control. if (!ControlRecord.ParentBodyComponentKey.IsValid() || ShouldComponentBeInSimulation(SolverComponentKey, ControlRecord.ParentBodyComponentKey)) { // Just make the record for now - it will be instantiated later ControlRecords.Add(ComponentKey, ControlRecord); } } } } } //====================================================================================================================== void FRigPhysicsSimulation::InitialiseJointRecords(const FRigPhysicsSolverComponent* SolverComponent) { ensure(JointRecords.IsEmpty()); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); if (!Hierarchy || !SolverComponent) { return; } FRigComponentKey SolverComponentKey = SolverComponent->GetKey(); TArray AllComponentKeys = Hierarchy->GetAllComponentKeys(); for (FRigComponentKey ComponentKey : AllComponentKeys) { if (const FRigPhysicsJointComponent* JointComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { // The authored body components may be blank, in which case we need to find the automatic ones. FRigJointRecord JointRecord; JointRecord.ParentBodyComponentKey = JointComponent->ParentBodyComponentKey; JointRecord.ChildBodyComponentKey = JointComponent->ChildBodyComponentKey; if (!JointRecord.ChildBodyComponentKey.IsValid()) { TArray SiblingComponentKeys = Hierarchy->GetComponentKeys(ComponentKey.ElementKey); for (FRigComponentKey SiblingComponentKey : SiblingComponentKeys) { if (ShouldComponentBeInSimulation(SolverComponentKey, SiblingComponentKey)) { JointRecord.ChildBodyComponentKey = SiblingComponentKey; break; } } } if (!JointRecord.ParentBodyComponentKey.IsValid()) { FRigElementKey ParentElementKey = Hierarchy->GetFirstParent(ComponentKey.ElementKey); TArray ParentComponentKeys = Hierarchy->GetComponentKeys(ParentElementKey); for (FRigComponentKey ParentComponentKey : ParentComponentKeys) { if (ShouldComponentBeInSimulation(SolverComponentKey, ParentComponentKey)) { JointRecord.ParentBodyComponentKey = ParentComponentKey; break; } } } if (ShouldComponentBeInSimulation(SolverComponentKey, JointRecord.ChildBodyComponentKey)) { if (ShouldComponentBeInSimulation(SolverComponentKey, JointRecord.ParentBodyComponentKey)) { // Just make the record for now - it will be instantiated later JointRecords.Add(ComponentKey, JointRecord); } } } } } //====================================================================================================================== void FRigPhysicsSimulation::InitialiseBodyRecords(const FRigPhysicsSolverComponent* SolverComponent) { ensure(BodyRecords.IsEmpty()); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); if (!Hierarchy || !SolverComponent) { return; } FRigComponentKey SolverComponentKey = SolverComponent->GetKey(); TArray AllComponentKeys = Hierarchy->GetAllComponentKeys(); // All the components in this simulation TArray UnsortedBodyComponentKeys; for (FRigComponentKey ComponentKey : AllComponentKeys) { if (ShouldComponentBeInSimulation(SolverComponentKey, ComponentKey)) { // Just make the record for now - it will be instantiated later BodyRecords.Add(ComponentKey); UnsortedBodyComponentKeys.Add(ComponentKey); } } // Sort the component keys according to the traversal of their element (i.e. from root to leaf) SortedBodyComponentKeys.Empty(UnsortedBodyComponentKeys.Num()); Hierarchy->Traverse([this, &UnsortedBodyComponentKeys](FRigBaseElement* Element, bool& bContinue) { const FRigElementKey& Key = Element->GetKey(); for (const FRigComponentKey& ComponentKey : UnsortedBodyComponentKeys) { if (ComponentKey.ElementKey == Key) { SortedBodyComponentKeys.Push(ComponentKey); } } }); } //====================================================================================================================== void FRigPhysicsSimulation::DestroyPhysicsSimulation() { for (TPair& BodyRecordPair : BodyRecords) { FRigBodyRecord& Record = BodyRecordPair.Value; if (Record.ActorHandle) { Simulation->DestroyActor(Record.ActorHandle); } } BodyRecords.Empty(); for (TPair& JointRecordPair : JointRecords) { FRigJointRecord& Record = JointRecordPair.Value; if (Record.JointHandle) { Simulation->DestroyJoint(Record.JointHandle); } } JointRecords.Empty(); for (TPair& ControlRecordPair : ControlRecords) { FRigControlRecord& Record = ControlRecordPair.Value; if (Record.JointHandle) { Simulation->DestroyJoint(Record.JointHandle); } } ControlRecords.Empty(); if (CollisionActorHandle) { Simulation->DestroyActor(CollisionActorHandle); } CollisionActorHandle = nullptr; if (SimulationActorHandle) { Simulation->DestroyActor(SimulationActorHandle); } SimulationActorHandle = nullptr; Simulation.Reset(); } //====================================================================================================================== static void SetCommonProperties(const FRigPhysicsCollisionShape& Shape, FKShapeElem& ShapeElem) { ShapeElem.RestOffset = Shape.RestOffset; ShapeElem.SetName(Shape.Name); ShapeElem.SetContributeToMass(Shape.bContributeToMass); #ifdef PER_SHAPE_COLLISION // Note that FKShapeElem supports enabling/disabling collision per shape, but this is discarded by the immediate // solver. ShapeElem.SetCollisionEnabled(Shape.CollisionEnabled); #endif } //====================================================================================================================== static Chaos::EJointMotionType GetMotionTypeFromLimitValue(const float Value) { if (Value > 0.0f) { return Chaos::EJointMotionType::Limited; } else if (Value < 0.0f) { return Chaos::EJointMotionType::Free; } else { return Chaos::EJointMotionType::Locked; } } //====================================================================================================================== static bool CreateGeometry( const FRigPhysicsCollision& Collision, const FRigPhysicsDynamics* Dynamics, const Chaos::FReal Density, Chaos::FReal& OutMass, Chaos::FVec3& OutInertia, Chaos::FRigidTransform3& OutCoMTransform, Chaos::FImplicitObjectPtr& OutGeom, TArray>& OutShapes) { using namespace Chaos; OutMass = 0.0f; OutInertia = FVector::ZeroVector; OutCoMTransform = FTransform::Identity; // Set the filter to collide with everything (we use a broad phase that only contains particle // pairs that are explicitly set to collide) FBodyCollisionData BodyCollisionData; // @todo(chaos): we need an API for setting up filters BodyCollisionData.CollisionFilterData.SimFilter.Word1 = 0xFFFF; BodyCollisionData.CollisionFilterData.SimFilter.Word3 = 0xFFFF; // See FBodyInstance::BuildBodyCollisionFlags BodyCollisionData.CollisionFlags.bEnableQueryCollision = false; BodyCollisionData.CollisionFlags.bEnableSimCollisionSimple = true; BodyCollisionData.CollisionFlags.bEnableSimCollisionComplex = false; BodyCollisionData.CollisionFlags.bEnableProbeCollision = false; FKAggregateGeom AggGeom; for (const FRigPhysicsCollisionBox& Shape : Collision.Boxes) { FKBoxElem Elem(Shape.Extents.X, Shape.Extents.Y, Shape.Extents.Z); SetCommonProperties(Shape, Elem); Elem.Center = Shape.TM.GetTranslation(); Elem.Rotation = Shape.TM.Rotator(); AggGeom.BoxElems.Add(Elem); } for (const FRigPhysicsCollisionSphere& Shape : Collision.Spheres) { FKSphereElem Elem(Shape.Radius); SetCommonProperties(Shape, Elem); Elem.Center = Shape.TM.GetTranslation(); // Note that there is no rotation AggGeom.SphereElems.Add(Elem); } for (const FRigPhysicsCollisionCapsule& Shape : Collision.Capsules) { FKSphylElem Elem(Shape.Radius, Shape.Length); SetCommonProperties(Shape, Elem); Elem.Center = Shape.TM.GetTranslation(); Elem.Rotation = Shape.TM.Rotator(); AggGeom.SphylElems.Add(Elem); } FGeometryAddParams AddParams; AddParams.CollisionData = BodyCollisionData; AddParams.CollisionTraceType = ECollisionTraceFlag::CTF_UseSimpleAsComplex; AddParams.Scale = FVector(1, 1, 1); AddParams.LocalTransform = FTransform::Identity; // How are these used? We will just set TM afterwards anyway AddParams.WorldTransform = FTransform::Identity; AddParams.Geometry = &AggGeom; TArray Geoms; FShapesArray Shapes; ChaosInterface::CreateGeometry(AddParams, Geoms, Shapes); if (Geoms.Num() == 0) { return false; } // Calculate mass properties, if we have dynamics if (Dynamics) { // Whether each shape contributes to mass. It would be easier if ComputeMassProperties knew // how to extract this info. Maybe it should be a flag in PerShapeData TArray bContributesToMass; bContributesToMass.Reserve(Shapes.Num()); for (int32 ShapeIndex = 0; ShapeIndex < Shapes.Num(); ++ShapeIndex) { const TUniquePtr& Shape = Shapes[ShapeIndex]; const FKShapeElem* ShapeElem = FChaosUserData::Get(Shape->GetUserData()); bool bHasMass = ShapeElem && ShapeElem->GetContributeToMass(); bContributesToMass.Add(bHasMass); } Chaos::FMassProperties MassProperties; ChaosInterface::CalculateMassPropertiesFromShapeCollection( MassProperties, Shapes, bContributesToMass, Density); OutMass = MassProperties.Mass; OutInertia = MassProperties.InertiaTensor.GetDiagonal(); OutCoMTransform = FTransform(MassProperties.RotationOfMass, MassProperties.CenterOfMass); } // If we have multiple root shapes, wrap them in a union if (Geoms.Num() == 1) { OutGeom = MoveTemp(Geoms[0]); } else { OutGeom = MakeImplicitObjectPtr(MoveTemp(Geoms)); } for (TUniquePtr& Shape : Shapes) { OutShapes.Emplace(MoveTemp(Shape)); } return true; } //====================================================================================================================== ImmediatePhysics::FActorHandle* FRigPhysicsSimulation::CreateBody( const FName BodyName, const FRigPhysicsCollision& Collision, const FRigPhysicsDynamics* Dynamics, const FPhysicsControlModifierData* BodyData, const FTransform& BodyRelSimSpaceTM) const { ImmediatePhysics::FActorSetup ActorSetup; if (Dynamics) { ActorSetup.ActorType = ImmediatePhysics::EActorType::DynamicActor; ActorSetup.bEnableGravity = true; ActorSetup.LinearDamping = Dynamics->LinearDamping; ActorSetup.AngularDamping = Dynamics->AngularDamping; } else { ActorSetup.ActorType = ImmediatePhysics::EActorType::KinematicActor; } if (BodyData) { ActorSetup.bUpdateKinematicFromSimulation = BodyData->bUpdateKinematicFromSimulation; } else { ActorSetup.bUpdateKinematicFromSimulation = false; } Chaos::FVec3 Inertia; Chaos::FRigidTransform3 CoMTransform; Chaos::FReal Mass; Chaos::FImplicitObjectPtr BodyGeom; TArray> BodyShapes; Chaos::FReal Density = Dynamics ? Dynamics->Density : 1.0f; // Convert from g/cm^3 to kg/cm^3 Density *= 1e-6; bool bGeometryCreated = CreateGeometry( Collision, Dynamics, Density, Mass, Inertia, CoMTransform, BodyGeom, BodyShapes); // We will have created with an arbitrary density - adjust to result in the desired mass. ActorSetup.Mass = Mass; ActorSetup.Inertia = Inertia; if (Mass > 0.0f && Dynamics && Dynamics->MassOverride > 0.0f) { ActorSetup.Mass = Dynamics->MassOverride; ActorSetup.Inertia = (Dynamics->MassOverride / Mass) * Inertia; } ActorSetup.Transform = BodyRelSimSpaceTM; ActorSetup.CoMTransform = CoMTransform; ActorSetup.Geometry = MoveTemp(BodyGeom); ActorSetup.Shapes = MoveTemp(BodyShapes); ActorSetup.Material = MakeUnique(); ActorSetup.Material->Friction = Collision.Material.Friction; ActorSetup.Material->StaticFriction = Collision.Material.Friction; ActorSetup.Material->Restitution = Collision.Material.Restitution; ActorSetup.Material->FrictionCombineMode = (Chaos::FChaosPhysicsMaterial::ECombineMode)Collision.Material.FrictionCombineMode; ActorSetup.Material->RestitutionCombineMode = (Chaos::FChaosPhysicsMaterial::ECombineMode)Collision.Material.RestitutionCombineMode; ImmediatePhysics::FActorHandle* ActorHandle = Simulation->CreateActor(MoveTemp(ActorSetup)); if (!ActorHandle) { UE_LOG(LogRigPhysics, Warning, TEXT("Unable to create body %s"), *BodyName.ToString()); return nullptr; } ActorHandle->SetName(BodyName); #if CHAOS_DEBUG_NAME if (Chaos::FGeometryParticleHandle* ParticleHandle = ActorHandle->GetParticle()) { ParticleHandle->SetDebugName(MakeShared(BodyName.ToString())); } #endif if (bGeometryCreated) { Simulation->AddToCollidingPairs(ActorHandle); if (Dynamics) { // Note that particles are always created disabled. They will simulate when disabled, // but won't collide! ActorHandle->SetEnabled(true); } } else { Simulation->SetHasCollision(ActorHandle, false); } return ActorHandle; } //====================================================================================================================== void FRigPhysicsSimulation::Instantiate( const FRigVMExecuteContext& ExecuteContext, const FRigPhysicsSolverComponent* SolverComponent) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_Instantiate); if (!bNeedToInstantiate || !OwningControlRig) { return; } // We need the simulation space in order to instantiate properly. This is not ideal, as we may // end up updating the simulation space data twice (thus inserting data into the history). This // shouldn't really matter as it will only be on the first step. UpdateSimulationSpaceStateAndCalculateData(ExecuteContext, SolverComponent, 0.0f); InitialiseSimulation(SolverComponent); InitialiseBodyRecords(SolverComponent); InitialiseJointRecords(SolverComponent); InitialiseControlRecords(SolverComponent); FRigPhysicsIgnorePairs IgnorePairs; InstantiatePhysicsBodies(SolverComponent, IgnorePairs); InstantiatePhysicsJoints(SolverComponent, IgnorePairs); InstantiateControls(SolverComponent, IgnorePairs); // This is done last as it applies IgnorePairs InstantiateSolverCollision(SolverComponent, IgnorePairs); bNeedToInstantiate = false; } //====================================================================================================================== ImmediatePhysics::FActorHandle* FRigPhysicsSimulation::GetActor(const FRigComponentKey& ComponentKey) const { if (const FRigBodyRecord* BodyRecord = BodyRecords.Find(ComponentKey)) { return BodyRecord->ActorHandle; } if (ComponentKey == PhysicsSolverComponentKey) { return SimulationActorHandle; } return nullptr; } //====================================================================================================================== void FRigPhysicsSimulation::InstantiateSolverCollision( const FRigPhysicsSolverComponent* SolverComponent, FRigPhysicsIgnorePairs& IgnorePairs) { // Optionally create an object to contain environment collision if (!SolverComponent->SolverSettings.Collision.IsEmpty()) { // When we make these additional collision shapes, their actors are all considered to be at // the origin, with the offsets being contained in the collision shapes. FTransform BodyRelSimSpaceTM = ConvertCollisionSpaceTransformToSimSpace( SolverComponent->SolverSettings, FTransform()); CollisionActorHandle = CreateBody( FName(TEXT("Environment")), SolverComponent->SolverSettings.Collision, nullptr, nullptr, BodyRelSimSpaceTM); } // Add no-collision pairs TArray ChaosIgnorePairs; for (const FRigPhysicsIgnorePair& IgnorePair : IgnorePairs) { ImmediatePhysics::FSimulation::FIgnorePair ChaosPair; ChaosPair.A = GetActor(IgnorePair.A); ChaosPair.B = GetActor(IgnorePair.B); if (ChaosPair.A && ChaosPair.B) { ChaosIgnorePairs.Add(ChaosPair); } } Simulation->SetIgnoreCollisionPairTable(ChaosIgnorePairs); } //====================================================================================================================== void FRigPhysicsSimulation::InstantiatePhysicsBodies( const FRigPhysicsSolverComponent* SolverComponent, FRigPhysicsIgnorePairs& IgnorePairs) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_InstantiatePhysicsBodies); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; const FRigPhysicsSimulationSpaceSettings& SimulationSpaceSettings = SolverComponent->SimulationSpaceSettings; for (TPair& BodyRecordPair : BodyRecords) { const FRigComponentKey& ComponentKey = BodyRecordPair.Key; FRigBodyRecord& Record = BodyRecordPair.Value; if (const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { const FRigPhysicsCollision& Collision = PhysicsComponent->Collision; const FRigPhysicsDynamics& Dynamics = PhysicsComponent->Dynamics; const FPhysicsControlModifierData& BodyData = PhysicsComponent->BodyData; FRigElementKey SourceKey = PhysicsComponent->BodySolverSettings.SourceBone; if (!SourceKey.IsValid()) { SourceKey = ComponentKey.ElementKey; } // What should we do if the key is not valid? if (SourceKey.IsValid()) { FTransform SourceComponentSpaceTM = Hierarchy->GetGlobalTransform(SourceKey); const FTransform SourceSimulationSpaceTM = ConvertComponentSpaceTransformToSimSpace(SolverComponent->SolverSettings, SourceComponentSpaceTM); Record.ActorHandle = CreateBody( ComponentKey.ElementKey.Name, Collision, &Dynamics, &BodyData, SourceSimulationSpaceTM); } Record.TargetElementKey = PhysicsComponent->BodySolverSettings.TargetBone; if (!Record.TargetElementKey.IsValid()) { Record.TargetElementKey = ComponentKey.ElementKey; } for (const FRigComponentKey& NoCollisionKey : PhysicsComponent->NoCollisionBodies) { IgnorePairs.Add(FRigPhysicsIgnorePair(ComponentKey, NoCollisionKey)); } } } } //====================================================================================================================== void FRigPhysicsSimulation::InstantiatePhysicsJoints( const FRigPhysicsSolverComponent* SolverComponent, FRigPhysicsIgnorePairs& IgnorePairs) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_InstantiatePhysicsJoints); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; const FRigPhysicsSimulationSpaceSettings& SimulationSpaceSettings = SolverComponent->SimulationSpaceSettings; // Once all the bodies are created, we can make their physics joints for (TPair& JointRecordPair : JointRecords) { const FRigComponentKey& JointComponentKey = JointRecordPair.Key; FRigJointRecord& JointRecord = JointRecordPair.Value; if (const FRigPhysicsJointComponent* PhysicsJointComponent = Cast( Hierarchy->FindComponent(JointComponentKey))) { const FRigPhysicsJointData& JointData = PhysicsJointComponent->JointData; FRigComponentKey ChildBodyComponentKey = JointRecord.ChildBodyComponentKey; FRigElementKey ChildBoneKey = ChildBodyComponentKey.ElementKey; FRigComponentKey ParentBodyComponentKey = JointRecord.ParentBodyComponentKey; FRigElementKey ParentBoneKey = ParentBodyComponentKey.ElementKey; // Joints require both parent and child to exist const FRigPhysicsBodyComponent* ChildPhysicsComponent = Cast(Hierarchy->FindComponent(ChildBodyComponentKey)); FRigBodyRecord* ChildBodyRecord = BodyRecords.Find(ChildBodyComponentKey); ImmediatePhysics::FActorHandle* ChildActorHandle = ChildBodyRecord ? ChildBodyRecord->ActorHandle : nullptr; if (!ChildActorHandle) { continue; } const FRigPhysicsBodyComponent* ParentPhysicsComponent = Cast(Hierarchy->FindComponent(ParentBodyComponentKey)); FRigBodyRecord* ParentBodyRecord = BodyRecords.Find(ParentBodyComponentKey); ImmediatePhysics::FActorHandle* ParentActorHandle = ParentBodyRecord ? ParentBodyRecord->ActorHandle : nullptr; if (!ParentActorHandle) { continue; } // Make the physics joint (joint constraint). // UE likes to treat Body1 (index 0) as the child, and Body2 (index 1) as the parent { const FTransform ParentCoMTransform = ParentActorHandle->GetLocalCoMTransform(); const FTransform ChildCoMTransform = ChildActorHandle->GetLocalCoMTransform(); Chaos::FPBDJointSettings JointSettings; if (JointData.bAutoCalculateChildOffset) { JointSettings.ConnectorTransforms[0] = FTransform(); } JointSettings.ConnectorTransforms[0] = JointData.ExtraChildOffset * JointSettings.ConnectorTransforms[0]; if (JointData.bAutoCalculateParentOffset) { JointSettings.ConnectorTransforms[1] = Hierarchy->GetLocalTransform(ChildBoneKey, true); } JointSettings.ConnectorTransforms[1] = JointData.ExtraParentOffset * JointSettings.ConnectorTransforms[1]; ImmediatePhysics::UpdateJointSettingsFromLinearConstraint(JointData.LinearConstraint, JointSettings); ImmediatePhysics::UpdateJointSettingsFromConeConstraint(JointData.ConeConstraint, JointSettings); ImmediatePhysics::UpdateJointSettingsFromTwistConstraint(JointData.TwistConstraint, JointSettings); // The physics setting is backwards, because we can't enable collision on bodies that // are set to not collide for other reasons. JointSettings.bCollisionEnabled = !JointData.bDisableCollision; JointSettings.bProjectionEnabled = JointData.LinearProjectionAmount > 0.0f || JointData.AngularProjectionAmount > 0.0f; JointSettings.AngularProjection = JointData.AngularProjectionAmount; JointSettings.LinearProjection = JointData.LinearProjectionAmount; JointSettings.ParentInvMassScale = JointData.ParentInverseMassScale; JointSettings.bUseLinearSolver = SolverSettings.bUseLinearJointSolver; JointRecord.JointHandle = Simulation->CreateJoint( ImmediatePhysics::FJointSetup(JointSettings, ChildActorHandle, ParentActorHandle)); if (Chaos::FPBDJointConstraintHandle* Constraint = JointRecord.JointHandle->GetConstraint()) { Chaos::FPBDJointSettings Settings = Constraint->GetSettings(); if (!Settings.bCollisionEnabled) { IgnorePairs.Add(FRigPhysicsIgnorePair( JointRecord.ChildBodyComponentKey, JointRecord.ParentBodyComponentKey)); } } } } } } //====================================================================================================================== void FRigPhysicsSimulation::InstantiateControls( const FRigPhysicsSolverComponent* SolverComponent, FRigPhysicsIgnorePairs& IgnorePairs) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_InstantiateControls); const URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); for (TPair& ControlRecordPair : ControlRecords) { const FRigComponentKey& ComponentKey = ControlRecordPair.Key; FRigControlRecord& ControlRecord = ControlRecordPair.Value; if (const FRigPhysicsControlComponent* ControlComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { if (FRigBodyRecord* ChildBodyRecord = BodyRecords.Find(ControlRecord.ChildBodyComponentKey)) { // this can be nullptr - just means a global control FRigBodyRecord* ParentBodyRecord = BodyRecords.Find(ControlRecord.ParentBodyComponentKey); ImmediatePhysics::FActorHandle* const ChildBodyHandle = ChildBodyRecord->ActorHandle; ImmediatePhysics::FActorHandle* const ParentBodyHandle = ParentBodyRecord ? ParentBodyRecord->ActorHandle : SimulationActorHandle; // This handles nullptrs. The constraint is created disabled - it will be updated in pre-physics ControlRecord.JointHandle = CreatePhysicsJoint(Simulation.Get(), ChildBodyHandle, ParentBodyHandle); if (!ControlRecord.JointHandle) { UE_LOG(LogRigPhysics, Warning, TEXT("Unable to create control constraint for %s"), *ControlComponent->GetKey().ToString()); } if (ControlComponent->ControlData.bDisableCollision) { IgnorePairs.Add(FRigPhysicsIgnorePair( ControlRecord.ChildBodyComponentKey, ControlRecord.ParentBodyComponentKey)); } } } } } //====================================================================================================================== void FRigPhysicsSimulation::UpdateBodyRecordPrePhysics( const FRigPhysicsSolverComponent* SolverComponent, const float DeltaTime, FRigBodyRecord& Record, const FRigPhysicsBodyComponent* PhysicsComponent) { URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); if (!Record.ActorHandle || !Hierarchy) { return; } const FRigComponentKey ComponentKey = PhysicsComponent->GetKey(); // Shuffle the record data Record.PrevSourceComponentSpaceVelocity = Record.SourceComponentSpaceVelocity; Record.PrevSourceComponentSpaceAngularVelocity = Record.SourceComponentSpaceAngularVelocity; Record.PrevSourceComponentSpaceTM = Record.SourceComponentSpaceTM; FRigElementKey SourceKey = PhysicsComponent->BodySolverSettings.SourceBone; if (!SourceKey.IsValid()) { SourceKey = ComponentKey.ElementKey; } if (SourceKey.IsValid()) { Record.SourceComponentSpaceTM = Hierarchy->GetGlobalTransform(SourceKey); } if (UpdateCounter == PreviousUpdateCounter + 1) { Record.SourceComponentSpaceVelocity = UE::PhysicsControl::CalculateLinearVelocity( Record.PrevSourceComponentSpaceTM.GetTranslation(), Record.SourceComponentSpaceTM.GetTranslation(), DeltaTime); Record.SourceComponentSpaceAngularVelocity = UE::PhysicsControl::CalculateAngularVelocity( Record.PrevSourceComponentSpaceTM.GetRotation(), Record.SourceComponentSpaceTM.GetRotation(), DeltaTime); } else { Record.PrevSourceComponentSpaceTM = Record.SourceComponentSpaceTM; Record.SourceComponentSpaceVelocity = FVector::ZeroVector; Record.SourceComponentSpaceAngularVelocity = FVector::ZeroVector; } } //====================================================================================================================== void FRigPhysicsSimulation::UpdateBodyPrePhysics( const FRigVMExecuteContext& ExecuteContext, const FRigPhysicsSolverComponent* SolverComponent, const FRigBodyRecord& Record, const FRigPhysicsBodyComponent* PhysicsComponent) { const FRigComponentKey ComponentKey = PhysicsComponent->GetKey(); FPhysicsControlModifierData BodyData = PhysicsComponent->BodyData; ERigPhysicsKinematicTargetSpace KinematicTargetSpace = PhysicsComponent->KinematicTargetSpace; if (SolverComponent->TrackInputCounter > 0) { BodyData.MovementType = EPhysicsMovementType::Kinematic; KinematicTargetSpace = ERigPhysicsKinematicTargetSpace::IgnoreTarget; } UpdateBodyFromModifierData( Record.ActorHandle, Simulation.Get(), BodyData, SimulationSpaceData.Gravity); if (Record.ActorHandle->GetIsKinematic()) { // Get the target in component space, and then convert it into sim space if necessary. // If the target is already in component space, then that's all we need FTransform KinematicTargetCS; switch (PhysicsComponent->KinematicTargetSpace) { case ERigPhysicsKinematicTargetSpace::Component: { KinematicTargetCS = PhysicsComponent->KinematicTarget; break; } case ERigPhysicsKinematicTargetSpace::World: { // Record.KinematicTarget * SimulationSpaceState.ComponentTM.Inverse(); KinematicTargetCS = PhysicsComponent->KinematicTarget.GetRelativeTransform( SimulationSpaceState.ComponentTM); break; } default: { // All the other options are relative to a bone, so the first task is to get // that, which will be in component space FRigElementKey SourceKey = PhysicsComponent->BodySolverSettings.SourceBone; if (!SourceKey.IsValid()) { SourceKey = ComponentKey.ElementKey; } if (SourceKey.IsValid()) { switch (PhysicsComponent->KinematicTargetSpace) { case ERigPhysicsKinematicTargetSpace::OffsetInBoneSpace: { KinematicTargetCS = PhysicsComponent->KinematicTarget * Record.SourceComponentSpaceTM.ToTransform(); break; } case ERigPhysicsKinematicTargetSpace::OffsetInWorldSpace: { // Convert the bone to WS, apply the target, and convert back FTransform BoneWS = Record.SourceComponentSpaceTM.ToTransform() * SimulationSpaceState.ComponentTM; FTransform KinematicTargetWS = BoneWS * PhysicsComponent->KinematicTarget; // Danny TODO figure out which of the GetRelativeTransform versions this is KinematicTargetCS = SimulationSpaceState.ComponentTM.Inverse() * KinematicTargetWS; break; } case ERigPhysicsKinematicTargetSpace::OffsetInComponentSpace: { KinematicTargetCS = Record.SourceComponentSpaceTM.ToTransform() * PhysicsComponent->KinematicTarget; break; } case ERigPhysicsKinematicTargetSpace::IgnoreTarget: { KinematicTargetCS = Record.SourceComponentSpaceTM.ToTransform(); break; } default: { UE_CONTROLRIG_RIGUNIT_REPORT_ERROR(TEXT("Kinematic target space is not valid")); } } } } } FTransform KinematicTargetTM = ConvertComponentSpaceTransformToSimSpace( SolverComponent->SolverSettings, KinematicTargetCS); Record.ActorHandle->SetKinematicTarget(KinematicTargetTM); } else { // Danny TODO move damping into BodyData - any PhysicsControl system should be able to use it Record.ActorHandle->SetLinearDamping(PhysicsComponent->Dynamics.LinearDamping); Record.ActorHandle->SetAngularDamping(PhysicsComponent->Dynamics.AngularDamping); } } //====================================================================================================================== void FRigPhysicsSimulation::UpdateJointPrePhysics( FRigJointRecord& Record, const FRigPhysicsJointComponent* PhysicsJointComponent, const URigHierarchy* Hierarchy, const float DeltaTime) { // Now update the joint targets if (!Record.JointHandle) { return; } if (Chaos::FPBDJointConstraintHandle* Constraint = Record.JointHandle->GetConstraint()) { const FRigComponentKey ComponentKey = PhysicsJointComponent->GetKey(); // Set the drive strength etc const FRigPhysicsJointData& JointData = PhysicsJointComponent->JointData; const FRigPhysicsDriveData& DriveData = PhysicsJointComponent->DriveData; Chaos::FPBDJointSettings Settings = Constraint->GetSettings(); ImmediatePhysics::UpdateJointSettingsFromLinearDriveConstraint(DriveData.LinearDriveConstraint, Settings); ImmediatePhysics::UpdateJointSettingsFromAngularDriveConstraint(DriveData.AngularDriveConstraint, Settings); Constraint->SetSettings(Settings); // Now set the actual target if (Settings.AngularDriveStiffness.SquaredLength() > 0.0f || Settings.AngularDriveDamping.SquaredLength() > 0.0f || Settings.LinearDriveStiffness.SquaredLength() > 0.0f || Settings.LinearDriveDamping.SquaredLength() > 0.0f) { // Multiplier on the velocity calculated from the current and previous target // // Danny TODO surely this should always be taken from DriveData? Do we/can we // distinguish between manual and animation velocities? float TargetVelocityMultiplier = 1.0f; FRigElementKey ChildSourceKey; if (const FRigPhysicsBodyComponent* ChildPhysicsComponent = Cast( Hierarchy->FindComponent(Record.ChildBodyComponentKey))) { ChildSourceKey = ChildPhysicsComponent->BodySolverSettings.SourceBone; if (!ChildSourceKey.IsValid()) { ChildSourceKey = ComponentKey.ElementKey; } TargetVelocityMultiplier = DriveData.SkeletalAnimationVelocityMultiplier; } UE::PhysicsControl::FPosQuat DriveTargetTM; if (DriveData.bUseSkeletalAnimation) { FRigElementKey ParentSourceKey; if (const FRigPhysicsBodyComponent* ParentPhysicsComponent = Cast( Hierarchy->FindComponent(Record.ParentBodyComponentKey))) { ParentSourceKey = ParentPhysicsComponent->BodySolverSettings.SourceBone; if (!ParentSourceKey.IsValid()) { ParentSourceKey = Record.ParentBodyComponentKey.ElementKey; } } if (!ChildSourceKey.IsValid() || !ParentSourceKey.IsValid()) { return; } // Danny TODO now all transforms are being cached in the record, get them from there // rather than the hierarchy // Note that the drive operates between a parent and child part, so we // don't need to worry about global/component (etc) space. const FTransform ChildTM = Hierarchy->GetGlobalTransform(ChildSourceKey); const FTransform ParentTM = Hierarchy->GetGlobalTransform(ParentSourceKey); const UE::PhysicsControl::FPosQuat ComponentSpaceParentFrameTM = Settings.ConnectorTransforms[1] * ParentTM; const UE::PhysicsControl::FPosQuat ComponentSpaceChildFrameTM = Settings.ConnectorTransforms[0] * ChildTM; DriveTargetTM = ComponentSpaceParentFrameTM.Inverse() * ComponentSpaceChildFrameTM; } // Apply the offset in the frame of the (potentially animation) target DriveTargetTM = DriveTargetTM * UE::PhysicsControl::FPosQuat( DriveData.LinearDriveConstraint.PositionTarget, DriveData.AngularDriveConstraint.OrientationTarget.Quaternion()); Constraint->SetLinearDrivePositionTarget(DriveTargetTM.GetTranslation()); Constraint->SetAngularDrivePositionTarget(DriveTargetTM.GetRotation()); if (Record.PreviousDriveTargetUpdateCounter + 1 == UpdateCounter && TargetVelocityMultiplier > 0.0f) { if (DeltaTime > SMALL_NUMBER) { const UE::PhysicsControl::FPosQuat DriveTargetTMDelta = DriveTargetTM * Record.PreviousDriveTargetTM.Inverse(); const FVector Velocity = DriveTargetTMDelta.GetTranslation() / DeltaTime; const FVector AngularVelocity = DriveTargetTMDelta.GetRotation().GetShortestArcWith( FQuat::Identity).ToRotationVector() / DeltaTime; if (!Velocity.ContainsNaN() && !AngularVelocity.ContainsNaN()) { Constraint->SetLinearDriveVelocityTarget(Velocity * TargetVelocityMultiplier); Constraint->SetAngularDriveVelocityTarget(AngularVelocity * TargetVelocityMultiplier); } } } else { Constraint->SetLinearDriveVelocityTarget(Chaos::FVec3(0)); Constraint->SetAngularDriveVelocityTarget(Chaos::FVec3(0)); } Record.PreviousDriveTargetUpdateCounter = UpdateCounter; Record.PreviousDriveTargetTM = DriveTargetTM; } } } //====================================================================================================================== static UE::PhysicsControl::FPosQuat CalculateTargetTM( const URigHierarchy* Hierarchy, const Chaos::FPBDJointSettings& JointSettings, const FRigControlRecord& Record) { const FTransform ChildTM = Record.ChildBodyComponentKey.IsValid() ? Hierarchy->GetGlobalTransform(Record.ChildBodyComponentKey.ElementKey) : FTransform(); const UE::PhysicsControl::FPosQuat ChildTargetTM = UE::PhysicsControl::FPosQuat(ChildTM) * UE::PhysicsControl::FPosQuat(JointSettings.ConnectorTransforms[ConstraintChildIndex]); if (Record.ParentBodyComponentKey.IsValid()) { const FTransform ParentTM = Hierarchy->GetGlobalTransform(Record.ParentBodyComponentKey.ElementKey); const UE::PhysicsControl::FPosQuat ParentTargetTM = UE::PhysicsControl::FPosQuat(ParentTM) * UE::PhysicsControl::FPosQuat(JointSettings.ConnectorTransforms[ConstraintParentIndex]); return ParentTargetTM.Inverse() * ChildTargetTM; } return ChildTargetTM; } //====================================================================================================================== void FRigPhysicsSimulation::CheckForResetsPrePhysics( FRigPhysicsSolverComponent* SolverComponent, const float DeltaTime) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_CheckForResetsPrePhysics); URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; double SpeedThresholdForResetSquared = SolverSettings.KinematicSpeedThresholdForReset > 0 ? FMath::Square(SolverSettings.KinematicSpeedThresholdForReset) : TNumericLimits::Max(); double AccelerationThresholdForResetSquared = SolverSettings.KinematicAccelerationThresholdForReset > 0 ? FMath::Square(SolverSettings.KinematicAccelerationThresholdForReset) : TNumericLimits::Max(); if (SpeedThresholdForResetSquared == TNumericLimits::Max() && AccelerationThresholdForResetSquared == TNumericLimits::Max()) { return; } double HighestSpeedSq = -1.0; double HighestAccelerationSq = -1.0; for (TPair& BodyRecordPair : BodyRecords) { const FRigComponentKey& ComponentKey = BodyRecordPair.Key; FRigBodyRecord& Record = BodyRecordPair.Value; if (Record.ActorHandle && Record.ActorHandle->GetIsKinematic()) { if (const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { if (PhysicsComponent->BodySolverSettings.bIncludeInChecksForReset) { const FVector Velocity = Record.SourceComponentSpaceVelocity; const FVector Acceleration = DeltaTime > SMALL_NUMBER && (UpdateCounter == PreviousUpdateCounter + 1) ? (Record.SourceComponentSpaceVelocity - Record.PrevSourceComponentSpaceVelocity) / DeltaTime : FVector::ZeroVector; HighestSpeedSq = FMath::Max(HighestSpeedSq, Velocity.SquaredLength()); HighestAccelerationSq = FMath::Max(HighestAccelerationSq, Acceleration.SquaredLength()); } } } } if (HighestSpeedSq > SpeedThresholdForResetSquared || HighestAccelerationSq > AccelerationThresholdForResetSquared) { if (HighestSpeedSq > SpeedThresholdForResetSquared) { UE_LOG(LogRigPhysics, Log, TEXT("Speed %f triggered reset in %s"), FMath::Sqrt(HighestSpeedSq), *OwningControlRig->GetName()); } if (HighestAccelerationSq > AccelerationThresholdForResetSquared) { UE_LOG(LogRigPhysics, Log, TEXT("Acceleration %f triggered reset in %s"), FMath::Sqrt(HighestAccelerationSq), *OwningControlRig->GetName()); } SolverComponent->TrackInputCounter = FMath::Max(SolverComponent->TrackInputCounter, 3); } } //====================================================================================================================== void FRigPhysicsSimulation::UpdatePrePhysics( const FRigVMExecuteContext& ExecuteContext, FRigPhysicsSolverComponent* SolverComponent, const float DeltaTime) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_UpdatePrePhysics); URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); if (CollisionActorHandle) { FTransform BodyRelSimSpaceTM = ConvertCollisionSpaceTransformToSimSpace( SolverComponent->SolverSettings, FTransform()); CollisionActorHandle->SetKinematicTarget(BodyRelSimSpaceTM); } for (TPair& BodyRecordPair : BodyRecords) { const FRigComponentKey& ComponentKey = BodyRecordPair.Key; FRigBodyRecord& Record = BodyRecordPair.Value; if (const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { UpdateBodyRecordPrePhysics(SolverComponent, DeltaTime, Record, PhysicsComponent); } } CheckForResetsPrePhysics(SolverComponent, DeltaTime); if (SolverComponent->TrackInputCounter > 0) { UE_LOG(LogRigPhysics, Log, TEXT("Forcing tracking (counter = %d) of input for %s"), SolverComponent->TrackInputCounter, *OwningControlRig->GetName()); } { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_UpdateBodiesPrePhysics); for (TPair& BodyRecordPair : BodyRecords) { const FRigComponentKey& ComponentKey = BodyRecordPair.Key; FRigBodyRecord& Record = BodyRecordPair.Value; if (const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { UpdateBodyPrePhysics(ExecuteContext, SolverComponent, Record, PhysicsComponent); } } } { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_UpdateJointsPrePhysics); for (TPair& JointRecordPair : JointRecords) { const FRigComponentKey& ComponentKey = JointRecordPair.Key; FRigJointRecord& JointRecord = JointRecordPair.Value; if (const FRigPhysicsJointComponent* JointComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { UpdateJointPrePhysics(JointRecord, JointComponent, Hierarchy, DeltaTime); } } } { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_UpdateControlsPrePhysics); for (TPair& ControlRecordPair : ControlRecords) { const FRigComponentKey& ComponentKey = ControlRecordPair.Key; FRigControlRecord& ControlRecord = ControlRecordPair.Value; if (const FRigPhysicsControlComponent* ControlComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { if (ControlComponent->ControlData.bEnabled) { UpdateControlPrePhysics(ControlRecord, ControlComponent, SolverComponent, Hierarchy, DeltaTime); } SetPhysicsJointEnabled(ControlRecord.JointHandle, ControlComponent->ControlData.bEnabled); } } } } //====================================================================================================================== void FRigPhysicsSimulation::UpdateControlPrePhysics( FRigControlRecord& ControlRecord, const FRigPhysicsControlComponent* ControlComponent, const FRigPhysicsSolverComponent* SolverComponent, const URigHierarchy* Hierarchy, const float DeltaTime) { const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; ImmediatePhysics::FJointHandle* JointHandle = ControlRecord.JointHandle; if (!JointHandle) { return; } Chaos::FPBDJointConstraintHandle* Constraint = JointHandle->GetConstraint(); if (!Constraint) { return; } float ThisDeltaTime = DeltaTime; if (ControlRecord.PreviousTargetUpdateCounter + 1 != UpdateCounter) { // If we missed some intermediate updates, then we don't want to use the previous // positions etc to calculate velocities. This will mean velocity/damping will be // incorrect for one frame, but that's probably OK. ThisDeltaTime = 0.0f; } Constraint->SetCollisionEnabled(!ControlComponent->ControlData.bDisableCollision); Constraint->SetParentInvMassScale(ControlComponent->ControlData.bOnlyControlChildObject ? 0 : 1); const Chaos::FPBDJointSettings& JointSettings = Constraint->GetSettings(); if (UpdateDriveSpringDamperSettings( JointHandle, JointSettings, ControlComponent->ControlData, ControlComponent->ControlMultiplier)) { const ImmediatePhysics::FActorHandle* ChildActorHandle = JointHandle->GetActorHandles()[ConstraintChildIndex]; const ImmediatePhysics::FActorHandle* ParentActorHandle = JointHandle->GetActorHandles()[ConstraintParentIndex]; if (ChildActorHandle && ParentActorHandle) { // TODO // - cache settings / previous input parameters to avoid unnecessary repeating // calculations and making physics API calls every update. // Update the target point on the child Constraint->SetChildConnectorLocation( ControlComponent->ControlData.GetControlPoint(ChildActorHandle)); FTransform TargetTM( ControlComponent->ControlTarget.TargetOrientation, ControlComponent->ControlTarget.TargetPosition); if (ControlComponent->ControlData.bUseSkeletalAnimation) { const FTransform ComponentSpaceAnimTargetTM = CalculateTargetTM( Hierarchy, JointSettings, ControlRecord).ToTransform(); const FTransform SimSpaceAnimTargetTM = ConvertComponentSpaceTransformToSimSpace( SolverSettings, ComponentSpaceAnimTargetTM); TargetTM = TargetTM * SimSpaceAnimTargetTM; } else { TargetTM = ConvertComponentSpaceTransformToSimSpace(SolverSettings, TargetTM); } Constraint->SetLinearDrivePositionTarget(TargetTM.GetTranslation()); Constraint->SetAngularDrivePositionTarget(TargetTM.GetRotation()); if ((ThisDeltaTime * ControlComponent->ControlData.LinearTargetVelocityMultiplier) != 0) { FVector Velocity = (TargetTM.GetTranslation() - ControlRecord.PreviousTargetTM.GetTranslation()) / ThisDeltaTime; Constraint->SetLinearDriveVelocityTarget( Velocity * ControlComponent->ControlData.LinearTargetVelocityMultiplier); } else { Constraint->SetLinearDriveVelocityTarget(Chaos::FVec3(0)); } if ((ThisDeltaTime * ControlComponent->ControlData.AngularTargetVelocityMultiplier) != 0) { // Note that quats multiply in the opposite order to TMs, and must be in the same hemisphere. const FQuat Q = TargetTM.GetRotation(); FQuat PrevQ = ControlRecord.PreviousTargetTM.GetRotation(); PrevQ.EnforceShortestArcWith(Q); const FQuat DeltaQ = Q * PrevQ.Inverse(); const FVector AngularVelocity = DeltaQ.ToRotationVector() / ThisDeltaTime; Constraint->SetAngularDriveVelocityTarget( AngularVelocity * ControlComponent->ControlData.AngularTargetVelocityMultiplier); } else { Constraint->SetAngularDriveVelocityTarget(Chaos::FVec3(0)); } ControlRecord.PreviousTargetTM = TargetTM; ControlRecord.PreviousTargetUpdateCounter = UpdateCounter; } else { // Note that if we don't have any strength, then we don't calculate the targets. // However, make sure that we don't apply velocities using the wrong calculation // when the strength/damping is increased in the future } } } //====================================================================================================================== // Note that we read back into a target bone, which may have been specified explicitly, or will // otherwise default to the physics element parent. void FRigPhysicsSimulation::UpdatePostPhysics( FRigPhysicsSolverComponent* SolverComponent, const float Alpha, const float DeltaTime) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_UpdatePostPhysics); if (Alpha == 0.0f) { return; } const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; bool bGotInvalidSimulationData = false; URigHierarchy* Hierarchy = OwningControlRig->GetHierarchy(); double PositionThresholdForResetSquared = SolverSettings.PositionThresholdForReset > 0 ? FMath::Square(SolverSettings.PositionThresholdForReset) : TNumericLimits::Max(); double HighestPosition = -1.0; // Traverse using the sorted keys for (const FRigComponentKey& ComponentKey : SortedBodyComponentKeys) { FRigBodyRecord& Record = *BodyRecords.Find(ComponentKey); if (Record.ActorHandle && Record.TargetElementKey.IsValid()) { // Check the simulation output FTransform SimSpaceTM = Record.ActorHandle->GetWorldTransform(); double DistSq = SimSpaceTM.GetTranslation().SquaredLength(); if (!SimSpaceTM.IsValid() || DistSq > PositionThresholdForResetSquared) { HighestPosition = FMath::Max(HighestPosition, SimSpaceTM.GetTranslation().Length()); bGotInvalidSimulationData = true; } // Calculate the target TM even if we're going to reset - it's likely useful for // debugging (and this should be rare!) Record.TargetComponentSpaceTM = ConvertSimSpaceTransformToComponentSpace( SolverSettings, SimSpaceTM); if (Alpha < 0.999f) { // Danny TODO Note that this uses Alpha to blend in component space. // This can cause joint separation. We probably want an option to blend // in local (joint) space, perhaps splitting the alpha into orientation // and position. FTransform CurrentTM = Record.SourceComponentSpaceTM.ToTransform(); FQuat TargetQ = FQuat::Slerp( CurrentTM.GetRotation(), Record.TargetComponentSpaceTM.GetRotation(), Alpha); FVector TargetT = FMath::Lerp( CurrentTM.GetTranslation(), Record.TargetComponentSpaceTM.GetTranslation(), Alpha); Record.TargetComponentSpaceTM.SetRotation(TargetQ); Record.TargetComponentSpaceTM.SetTranslation(TargetT); } } } if (bGotInvalidSimulationData) { if (HighestPosition > 0) { UE_LOG(LogRigPhysics, Log, TEXT("Position %f triggered teleport in %s - resetting pose"), HighestPosition, *OwningControlRig->GetName()); } // Avoid cached transforms being used in controls by bumping the update counter. UpdateCounter += 1; // Set this to 3 since it gets decremented at the end of the update, and we need it to take // effect at the start of the next update. SolverComponent->TrackInputCounter = FMath::Max(SolverComponent->TrackInputCounter, 3); } // If we found something invalid then we force the simulation to be as good as we can make it, // and we don't write back to the hierarchy. if (HighestPosition > 0) { UE_LOG(LogRigPhysics, Log, TEXT("Resetting state to input pose in %s"), *OwningControlRig->GetName()); for (TPair& BodyRecordPair : BodyRecords) { const FRigComponentKey& ComponentKey = BodyRecordPair.Key; const FRigBodyRecord& Record = BodyRecordPair.Value; if (const FRigPhysicsBodyComponent* PhysicsComponent = Cast( Hierarchy->FindComponent(ComponentKey))) { if (Record.ActorHandle && !Record.ActorHandle->GetIsKinematic()) { // Get the TM in component space, and then convert it into sim space. FRigElementKey SourceKey = PhysicsComponent->BodySolverSettings.SourceBone; if (!SourceKey.IsValid()) { SourceKey = ComponentKey.ElementKey; } if (SourceKey.IsValid()) { FTransform SourceComponentSpaceTM = Record.SourceComponentSpaceTM.ToTransform(); const FTransform SourceSimulationSpaceTM = ConvertComponentSpaceTransformToSimSpace(SolverSettings, SourceComponentSpaceTM); Record.ActorHandle->SetWorldTransform(SourceSimulationSpaceTM); } Record.ActorHandle->SetLinearVelocity(FVector::ZeroVector); Record.ActorHandle->SetAngularVelocity(FVector::ZeroVector); } } } } else { // All is good - write the transforms we cached for (const FRigComponentKey& ComponentKey : SortedBodyComponentKeys) { const FRigBodyRecord& Record = *BodyRecords.Find(ComponentKey); if (Record.ActorHandle && Record.TargetElementKey.IsValid()) { // Note that we set bAffectChildren = true (i.e. don't counter-animate // children), so that attached animation bones will follow physics, but we // rely on our bodies being sorted so we work out from the leaf nodes so as // not to disturb previously set bodies. Hierarchy->SetGlobalTransform(Record.TargetElementKey, Record.TargetComponentSpaceTM, false, true); } } } } //====================================================================================================================== void FRigPhysicsSimulation::StepSimulation( const FRigVMExecuteContext& ExecuteContext, FRigPhysicsSolverComponent* SolverComponent, const float DeltaTimeOverride, const float SimulationSpaceDeltaTimeOverride, const float Alpha) { DECLARE_SCOPE_HIERARCHICAL_COUNTER_FUNC() QUICK_SCOPE_CYCLE_COUNTER(STAT_RigPhysics_StepSimulation); // Increment the update counter at the start - and always update it so this tells us our "frame number". ++UpdateCounter; float PhysicsDeltaTime = ExecuteContext.GetDeltaTime(); if (DeltaTimeOverride > 0.0f) { PhysicsDeltaTime = DeltaTimeOverride; } else if (DeltaTimeOverride < 0.0f) { PhysicsDeltaTime = 0.0f; } float PhysicsSimulationSpaceDeltaTime = PhysicsDeltaTime; if (SimulationSpaceDeltaTimeOverride > 0.0f) { PhysicsSimulationSpaceDeltaTime = SimulationSpaceDeltaTimeOverride; } // We need to know about the simulation space etc before we can instantiate anything into the right place SimulationSpaceData = UpdateSimulationSpaceStateAndCalculateData( ExecuteContext, SolverComponent, PhysicsSimulationSpaceDeltaTime); // We instantiate when we do the first simulation - this makes sure any changes applied by // the user have been made. It also means there is no overhead if physics is never stepped. // However, there may be a hitch due to the creation, so it may also happen during construction. Instantiate(ExecuteContext, SolverComponent); // There is an early out if it's already been done if (!Simulation) { return; } const FRigPhysicsSolverSettings& SolverSettings = SolverComponent->SolverSettings; const FRigPhysicsSimulationSpaceSettings& SimulationSpaceSettings = SolverComponent->SimulationSpaceSettings; float FixedTimeStep = CVarControlRigPhysicsFixedTimeStepOverride.GetValueOnAnyThread() < 0 ? SolverSettings.FixedTimeStep : CVarControlRigPhysicsFixedTimeStepOverride.GetValueOnAnyThread(); int MaxTimeSteps = CVarControlRigPhysicsMaxTimeStepsOverride.GetValueOnAnyThread() < 0 ? SolverSettings.MaxTimeSteps : CVarControlRigPhysicsMaxTimeStepsOverride.GetValueOnAnyThread(); float MaxDeltaTime = CVarControlRigPhysicsMaxDeltaTimeOverride.GetValueOnAnyThread() < 0 ? SolverSettings.MaxDeltaTime : CVarControlRigPhysicsMaxDeltaTimeOverride.GetValueOnAnyThread(); // Set settings that might change Simulation->SetSolverSettings( FixedTimeStep, SolverSettings.CollisionBoundsExpansion, SolverSettings.MaxDepenetrationVelocity, SolverSettings.bUseLinearJointSolver, SolverSettings.PositionIterations, SolverSettings.VelocityIterations, SolverSettings.ProjectionIterations, SolverSettings.bUseManifolds); Simulation->SetUseMinStepTime(false); Simulation->SetUseFixedStepTolerance(false); Chaos::FCollisionDetectorSettings CollisionDetectorSettings = Simulation->GetCollisionDetectorSettings(); CollisionDetectorSettings.BoundsVelocityInflation = SolverSettings.BoundsVelocityMultiplier; CollisionDetectorSettings.MaxVelocityBoundsExpansion = SolverSettings.MaxVelocityBoundsExpansion; Simulation->SetCollisionDetectorSettings(CollisionDetectorSettings); // This gets reset to 100 after every simulation step! Simulation->SetMaxNumRollingAverageStepTimes(SolverSettings.MaxNumRollingAverageStepTimes); // Other settings - would normally be static (so Danny TODO move this) ChaosJointSolverSettings.bSolvePositionLast = SolverSettings.bSolveJointPositionsLast; ChaosJointSolverSettings.bSortEnabled = true; // Simulation space Chaos::FSimulationSpaceSettings ChaosSimulationSpaceSettings = Simulation->GetSimulationSpaceSettings(); ChaosSimulationSpaceSettings.bEnabled = (SimulationSpaceSettings.SpaceMovementAmount > 0.0f); ChaosSimulationSpaceSettings.ExternalLinearEtherDrag = SimulationSpaceSettings.ExternalLinearDrag; ChaosSimulationSpaceSettings.LinearVelocityAlpha = SimulationSpaceSettings.LinearDragMultiplier; ChaosSimulationSpaceSettings.AngularVelocityAlpha = SimulationSpaceSettings.AngularDragMultiplier; Simulation->SetSimulationSpaceSettings(ChaosSimulationSpaceSettings); Simulation->UpdateSimulationSpace( SimulationSpaceState.SimulationSpaceTM, SimulationSpaceSettings.SpaceMovementAmount * SimulationSpaceData.LinearVelocity, SimulationSpaceSettings.SpaceMovementAmount * SimulationSpaceData.AngularVelocity, SimulationSpaceSettings.SpaceMovementAmount * SimulationSpaceData.LinearAcceleration, SimulationSpaceSettings.SpaceMovementAmount * SimulationSpaceData.AngularAcceleration); // Only update if there is a delta time: // * We don't want to update our previous TMs and store the dt - because that would end up // implying infinite velocities // * We don't to update kinematic bodies with the new TMs because, since the simulated ones // won't move, that would break the pose. // * We can't actually simulate with dt = 0 if (PhysicsDeltaTime > 0.0f) { UpdatePrePhysics(ExecuteContext, SolverComponent, PhysicsDeltaTime); Simulation->Simulate( PhysicsDeltaTime, MaxDeltaTime, MaxTimeSteps, SimulationSpaceData.Gravity, &ChaosJointSolverSettings); PreviousUpdateCounter = UpdateCounter; } // Always do a read-back, even for zero Dt UpdatePostPhysics(SolverComponent, FMath::Clamp(Alpha, 0.0f, 1.0f), PhysicsDeltaTime); if (SolverComponent->TrackInputCounter > 0) { --SolverComponent->TrackInputCounter; } }