FieldSystemProxyHelper.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "Field/FieldSystem.h"
#include "Chaos/Array.h"
#include "Chaos/ConstraintHandle.h"
#include "Chaos/ParticleHandle.h"
#include "Chaos/Defines.h"
#include "Chaos/PBDRigidClusteringAlgo.h"
#include "Chaos/Particles.h"
#include <limits>
 
#include "GeometryCollectionPhysicsProxy.h"
 
namespace Chaos
{
    template <typename PhysicsProxy>
    static bool BuildFieldSamplePoints(
        PhysicsProxy* LocalProxy,
        Chaos::FPBDRigidsSolver* RigidSolver,
        const FFieldSystemCommand& FieldCommand, 
        FFieldExecutionDatas& ExecutionDatas,
        EFieldResolutionType& PrevResolutionType, 
        EFieldFilterType& PrevFilterType, 
        EFieldObjectType& PrevObjectType,
        EFieldPositionType& PrevPositionType)
    {
        if(!LocalProxy || !RigidSolver)
        {
            return false;
        }
        const EFieldResolutionType ResolutionType =
            FieldCommand.HasMetaData(FFieldSystemMetaData::EMetaType::ECommandData_ProcessingResolution) ?
            FieldCommand.GetMetaDataAs<FFieldSystemMetaDataProcessingResolution>( 
                FFieldSystemMetaData::EMetaType::ECommandData_ProcessingResolution)->ProcessingResolution :
            EFieldResolutionType::Field_Resolution_Minimal;
 
        EFieldFilterType FilterType = EFieldFilterType::Field_Filter_Max;
        EFieldObjectType ObjectType = EFieldObjectType::Field_Object_Max;
        EFieldPositionType PositionType = EFieldPositionType::Field_Position_Max;
 
        if (FieldCommand.HasMetaData(FFieldSystemMetaData::EMetaType::ECommandData_Filter))
        {
            const FFieldSystemMetaDataFilter* MetaDataFilter = FieldCommand.GetMetaDataAs<FFieldSystemMetaDataFilter>(FFieldSystemMetaData::EMetaType::ECommandData_Filter);
            FilterType = MetaDataFilter->FilterType;
            ObjectType = MetaDataFilter->ObjectType;
            PositionType = MetaDataFilter->PositionType;
        }
 
        TArray<Chaos::FGeometryParticleHandle*>& FilteredHandles = ExecutionDatas.ParticleHandles[(uint8)EFieldCommandHandlesType::FilteredHandles];
        TArray<Chaos::FGeometryParticleHandle*>& InsideHandles = ExecutionDatas.ParticleHandles[(uint8)EFieldCommandHandlesType::InsideHandles];
 
        //if (LocalProxy && ( (PrevResolutionType != ResolutionType) || (PrevFilterType != FilterType) || (PrevObjectType != ObjectType) || (PrevPositionType != PositionType) || FilteredHandles.Num() == 0))
        {
            if (FilterType != EFieldFilterType::Field_Filter_Max)
            {
                LocalProxy->GetFilteredParticleHandles(FilteredHandles, RigidSolver, FilterType, ObjectType);
            }
            else
            {
                LocalProxy->GetRelevantParticleHandles(FilteredHandles, RigidSolver, ResolutionType);
            }
 
            PrevResolutionType = ResolutionType;
            PrevFilterType = FilterType;
            PrevObjectType = ObjectType;
            PrevPositionType = PositionType;
 
            ExecutionDatas.SamplePositions.SetNum(FilteredHandles.Num(), EAllowShrinking::No);
            ExecutionDatas.SampleIndices.SetNum(FilteredHandles.Num(), EAllowShrinking::No);
            InsideHandles.SetNum(FilteredHandles.Num(), EAllowShrinking::No);
 
            auto FillExecutionDatas = [&ExecutionDatas,&FieldCommand,&InsideHandles](FVec3 SamplePosition, Chaos::FGeometryParticleHandle* ParticleHandle, int32& HandleIndex)
            {
                if (FPBDRigidClusteredParticleHandle* ClusterHandle = ParticleHandle->CastToClustered())
                {
                    // Disabled clustered particles that are driven by a parent, contain particle 
                    // positions in local space. The field system requires the transformation of 
                    // the disabled child particles into world space.
                    if (ClusterHandle->Disabled() == true)
                    {
                        FRigidTransform3 RelativeFrame = ClusterHandle->ChildToParent();
 
                        // go up the chain until no parent or a non-disabled parent ( should normally stop at a non disabled one )
                        FPBDRigidClusteredParticleHandle* ParentHandle = ClusterHandle->Parent();
                        while (ParentHandle && ParentHandle->Disabled())
                        {
                            RelativeFrame = RelativeFrame * ParentHandle->ChildToParent();
                            ParentHandle = ParentHandle->Parent();
                        }
 
                        if (ParentHandle && ParentHandle->Disabled() == false)
                        {
                            const FRigidTransform3 ParentWorldTM(ParentHandle->GetP(), ParentHandle->GetQ());
                            const FRigidTransform3 ChildFrame = RelativeFrame * ParentWorldTM;
                            SamplePosition = ChildFrame.GetTranslation();
                        }
                    }
                }
 
 
                if (FieldCommand.BoundingBox.IsInside(SamplePosition))
                {
                    ExecutionDatas.SamplePositions[HandleIndex] = SamplePosition;
                    ExecutionDatas.SampleIndices[HandleIndex] = FFieldContextIndex(HandleIndex, HandleIndex);
                    InsideHandles[HandleIndex] = ParticleHandle;
                    ++HandleIndex;
                }
            };
 
            int32 HandleIndex = 0;
            if (PositionType == EFieldPositionType::Field_Position_CenterOfMass)
            {
                for (int32 Idx = 0; Idx < FilteredHandles.Num(); ++Idx)
                {
                    if (Chaos::FPBDRigidParticleHandle* RigidHandle = FilteredHandles[Idx]->CastToRigidParticle())
                    {
                        const FVec3 SamplePosition = FParticleUtilities::GetCoMWorldPosition(RigidHandle);
                        FillExecutionDatas(SamplePosition, RigidHandle, HandleIndex);
                    }
                }
            }
            else
            {
                for (int32 Idx = 0; Idx < FilteredHandles.Num(); ++Idx)
                {
                    if (Chaos::FGeometryParticleHandle* FilteredHandle = FilteredHandles[Idx])
                    {
                        const FVec3& SamplePosition = FilteredHandle->GetX();
                        FillExecutionDatas(SamplePosition, FilteredHandle, HandleIndex);
                    }
                }
            }
            ExecutionDatas.SamplePositions.SetNum(HandleIndex, EAllowShrinking::No);
            ExecutionDatas.SampleIndices.SetNum(HandleIndex, EAllowShrinking::No);
            InsideHandles.SetNum(HandleIndex, EAllowShrinking::No);
        }
        return InsideHandles.Num() > 0;
    }
 
    static void InitDynamicStateResults(const TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, TArray<int32>& LocalResults)
    {
        for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
        {
            const Chaos::EObjectStateType InitState = (ParticleHandles[Index.Sample]->ObjectState() != Chaos::EObjectStateType::Uninitialized) ?
                ParticleHandles[Index.Sample]->ObjectState() : Chaos::EObjectStateType::Dynamic;
            LocalResults[Index.Result] = static_cast<int32>(InitState);
        }
    }
 
    static void InitActivateDisabledResults(const TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, TArray<int32>& LocalResults)
    {
        for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
        {
            Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
            if (RigidHandle)
            {
                LocalResults[Index.Result] = RigidHandle->Disabled();
            }
        }
    }
 
    static void SetParticleDynamicState(Chaos::FPBDRigidsSolver* RigidSolver,
        const Chaos::EObjectStateType FieldState, Chaos::FPBDRigidParticleHandle* RigidHandle)
    {
        const bool bIsGC = (RigidHandle->GetParticleType() == Chaos::EParticleType::GeometryCollection) ||
            (RigidHandle->GetParticleType() == Chaos::EParticleType::Clustered && !RigidHandle->CastToClustered()->InternalCluster());
 
        if (!bIsGC)
        {
            RigidSolver->GetEvolution()->SetParticleObjectState(RigidHandle, FieldState);
        }
        else
        {
            // @todo(chaos): this should also call Evolution.SetParticleObjectState, but to do that
            // we need to change how the ActiveParticlesArray is handled in RigidParticleSOAs.
            // Instead we manually do the parts of SetParticleObjectState that are still required 
            // while avoiding the SOA management.
            RigidHandle->SetObjectStateLowLevel(FieldState);
            if (!RigidHandle->Disabled())
            {
                RigidSolver->GetEvolution()->GetIslandManager().AddParticle(RigidHandle);
 
                // This is mainly just to refresh the views. Note that we do not want to enable disabled particles.
                RigidSolver->GetEvolution()->GetParticles().EnableParticle(RigidHandle);
            }
        }
    }
 
    static bool ReportDynamicStateResult(Chaos::FPBDRigidsSolver* RigidSolver,
        const Chaos::EObjectStateType FieldState, Chaos::FPBDRigidParticleHandle* RigidHandle,
        const bool HasInitialLinearVelocity, const Chaos::FVec3& InitialLinearVelocity,
        const bool HasInitialAngularVelocity, const Chaos::FVec3& InitialAngularVelocity)
    {
        const Chaos::EObjectStateType HandleState = RigidHandle->ObjectState();
 
        // Do we need to be sure the mass > 0 only for the dynamic state
        const bool bHasStateChanged = ((FieldState != Chaos::EObjectStateType::Dynamic) ||
            (FieldState == Chaos::EObjectStateType::Dynamic && RigidHandle->M() > FLT_EPSILON)) && (HandleState != FieldState);  
 
        if (bHasStateChanged)
        {
            SetParticleDynamicState(RigidSolver, FieldState, RigidHandle);
 
            if (FieldState == Chaos::EObjectStateType::Kinematic || FieldState == Chaos::EObjectStateType::Static)
            {
                RigidHandle->SetVf(Chaos::FVec3f(0));
                RigidHandle->SetWf(Chaos::FVec3f(0));
            }
            else if (FieldState == Chaos::EObjectStateType::Dynamic)
            {
                if (HasInitialLinearVelocity)
                {
                    RigidHandle->SetV(InitialLinearVelocity);
                }
                if (HasInitialAngularVelocity)
                {
                    RigidHandle->SetW(InitialAngularVelocity);
                }
            }
        }
        return bHasStateChanged;
    }
 
    static void UpdateSolverParticlesState(Chaos::FPBDRigidsSolver* RigidSolver, const TFieldArrayView<FFieldContextIndex>& UpdatedParticleIndices, const TArray<Chaos::FGeometryParticleHandle*>& Particles)
    {
        if (UpdatedParticleIndices.Num() > 0)
        {
            TSet<FPBDRigidClusteredParticleHandle*> TopParentClusters;
            for (const FFieldContextIndex& ParticleIndex: UpdatedParticleIndices)
            {
                if (FPBDRigidClusteredParticleHandle* ClusteredHandle = Particles[ParticleIndex.Sample]->CastToClustered())
                {
                    FPBDRigidClusteredParticleHandle* TopParent = ClusteredHandle;
                    while (FPBDRigidClusteredParticleHandle* DirectParent = TopParent->Parent())
                    {
                        TopParent = DirectParent;
                    }
                    TopParentClusters.Add(TopParent);
                }
            }
 
            const FRigidClustering& Clustering = RigidSolver->GetEvolution()->GetRigidClustering();
            for (FPBDRigidClusteredParticleHandle* TopParentClusteredHandle: TopParentClusters)
            {
                if (TopParentClusteredHandle && !TopParentClusteredHandle->Disabled())
                {
                    if (TopParentClusteredHandle->ClusterIds().NumChildren)
                    {
                        UpdateKinematicProperties(TopParentClusteredHandle, Clustering.GetChildrenMap(), *RigidSolver->GetEvolution());
                    }
                    else
                    {
                        // if the cluster is dynamic let's make sure we clear kinematic target to avoid animated ones to have their velocity reset by the kinematic target update
                        // for particles with children this is taken care in UpdateKinematicProperties
                        if (TopParentClusteredHandle->ObjectState() == EObjectStateType::Dynamic)
                        {
                            RigidSolver->GetEvolution()->SetParticleKinematicTarget(TopParentClusteredHandle, FKinematicTarget());
                        }
                    }
                }
            }
        }
    }
 
    static void UpdateMaterialSleepingThreshold(Chaos::FPBDRigidsSolver* RigidSolver, Chaos::FPBDRigidParticleHandle* RigidHandle, const Chaos::FReal ResultThreshold)
    {
        // if no per particle physics material is set, make one
        if (!RigidSolver->GetEvolution()->GetPerParticlePhysicsMaterial(RigidHandle).IsValid())
        {
            TUniquePtr<Chaos::FChaosPhysicsMaterial> NewMaterial = MakeUnique< Chaos::FChaosPhysicsMaterial>();
            NewMaterial->SleepingLinearThreshold = ResultThreshold;
            NewMaterial->SleepingAngularThreshold = ResultThreshold;
 
            RigidSolver->GetEvolution()->SetPhysicsMaterial(RigidHandle, MakeSerializable(NewMaterial));
            RigidSolver->GetEvolution()->SetPerParticlePhysicsMaterial(RigidHandle, NewMaterial);
        }
        else
        {
            const TUniquePtr<Chaos::FChaosPhysicsMaterial>& InstanceMaterial = RigidSolver->GetEvolution()->GetPerParticlePhysicsMaterial(RigidHandle);
 
            if (ResultThreshold != InstanceMaterial->DisabledLinearThreshold)
            {
                InstanceMaterial->SleepingLinearThreshold = ResultThreshold;
                InstanceMaterial->SleepingAngularThreshold = ResultThreshold;
            }
        }
    }
 
    static void UpdateMaterialDisableThreshold(Chaos::FPBDRigidsSolver* RigidSolver, Chaos::FPBDRigidParticleHandle* RigidHandle, const Chaos::FReal ResultThreshold)
    {
        // if no per particle physics material is set, make one
        if (!RigidSolver->GetEvolution()->GetPerParticlePhysicsMaterial(RigidHandle).IsValid())
        {
            TUniquePtr<Chaos::FChaosPhysicsMaterial> NewMaterial = MakeUnique< Chaos::FChaosPhysicsMaterial>();
            NewMaterial->DisabledLinearThreshold = ResultThreshold;
            NewMaterial->DisabledAngularThreshold = ResultThreshold;
 
            RigidSolver->GetEvolution()->SetPhysicsMaterial(RigidHandle, MakeSerializable(NewMaterial));
            RigidSolver->GetEvolution()->SetPerParticlePhysicsMaterial(RigidHandle, NewMaterial);
        }
        else
        {
            const TUniquePtr<Chaos::FChaosPhysicsMaterial>& InstanceMaterial = RigidSolver->GetEvolution()->GetPerParticlePhysicsMaterial(RigidHandle);
 
            if (ResultThreshold != InstanceMaterial->DisabledLinearThreshold)
            {
                InstanceMaterial->DisabledLinearThreshold = ResultThreshold;
                InstanceMaterial->DisabledAngularThreshold = ResultThreshold;
            }
        }
    }
 
    static void FieldIntegerParameterUpdate(Chaos::FPBDRigidsSolver* RigidSolver, const FFieldSystemCommand& FieldCommand,
        TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, 
        Chaos::FPBDPositionConstraints& PositionTarget,
        TMap<int32, int32>& TargetedParticles, TArray<int32>& FinalResults)
    {
        TFieldArrayView<int32> ResultsView(FinalResults, 0, FinalResults.Num());
 
        if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_DynamicState)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_DynamicState);
            {
                InitDynamicStateResults(ParticleHandles, FieldContext, FinalResults);
 
                static_cast<const FFieldNode<int32>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
 
                bool bHasStateChanged = false;
                
                const TFieldArrayView<FFieldContextIndex>& EvaluatedSamples = FieldContext.GetEvaluatedSamples();
                for (const FFieldContextIndex& Index : EvaluatedSamples)
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle)
                    {
                        const int32 CurrResult = ResultsView[Index.Result];
                        check(CurrResult < std::numeric_limits<int8>::max());
 
                        const int8 ResultState = static_cast<int8>(CurrResult);
                        bHasStateChanged |= ReportDynamicStateResult(RigidSolver, static_cast<Chaos::EObjectStateType>(ResultState), RigidHandle,
                            false, Chaos::FVec3(0), false, Chaos::FVec3(0));
                    }
                }
                if (bHasStateChanged)
                {
                    UpdateSolverParticlesState(RigidSolver, EvaluatedSamples, ParticleHandles);
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_ActivateDisabled)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_ActivateDisabled);
            {
                InitActivateDisabledResults(ParticleHandles, FieldContext, FinalResults);
 
                static_cast<const FFieldNode<int32>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && RigidHandle->Disabled() && ResultsView[Index.Result] == 0)
                    {
                        RigidSolver->GetEvolution()->EnableParticle(RigidHandle);
                        SetParticleDynamicState(RigidSolver, Chaos::EObjectStateType::Dynamic, RigidHandle);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_CollisionGroup)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_CollisionGroup);
            {
                static_cast<const FFieldNode<int32>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidClusteredParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToClustered();
                    if (RigidHandle)
                    {
                        RigidHandle->SetCollisionGroup(ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_PositionStatic)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_PositionStatic);
            {
                static_cast<const FFieldNode<int32>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidClusteredParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToClustered();
                    if (RigidHandle && ResultsView[Index.Result])
                    {
                        if (TargetedParticles.Contains(Index.Sample))
                        {
                            const int32 ConstraintIndex = TargetedParticles[Index.Sample];
                            PositionTarget.Replace(ConstraintIndex, ParticleHandles[Index.Sample]->GetX());
                        }
                        else
                        {
                            const int32 ConstraintIndex = PositionTarget.NumConstraints();
                            PositionTarget.AddConstraint(RigidHandle, RigidHandle->GetX());
                            TargetedParticles.Add(Index.Sample, ConstraintIndex);
                        }
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_DynamicConstraint)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_DynamicConstraint);
            {
                UE_LOG(LogChaos, Error, TEXT("Dynamic constraint target currently not supported by chaos"));
            }
        }
    }
 
    static void FieldScalarParameterUpdate(Chaos::FPBDRigidsSolver* RigidSolver, const FFieldSystemCommand& FieldCommand,
        TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, 
        Chaos::FPBDPositionConstraints& PositionTarget,
        TMap<int32, int32>& TargetedParticles, TArray<float>& FinalResults)
    {
        TFieldArrayView<float> ResultsView(FinalResults, 0, FinalResults.Num());
 
        if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_ExternalClusterStrain)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_ExternalClusterStrain);
            {
                FRigidClustering& RigidClustering = RigidSolver->GetEvolution()->GetRigidClustering();
                static_cast<const FFieldNode<float>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    const float ExternalStrainValue = ResultsView[Index.Result];
                    if (ExternalStrainValue > 0)
                    {
                        if (Chaos::FPBDRigidClusteredParticleHandle* ClusteredParticle = ParticleHandles[Index.Sample]->CastToClustered())
                        {
                            const FRealSingle CurrentExternalStrains = ClusteredParticle->GetExternalStrain();
                            RigidClustering.SetExternalStrain(ClusteredParticle, FMath::Max(CurrentExternalStrains, ExternalStrainValue));
                        }
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_Kill)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_Kill);
            {
                static_cast<const FFieldNode<float>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && ResultsView[Index.Result] > 0.0)
                    {
                        RigidSolver->GetEvolution()->DisableParticle(RigidHandle);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_SleepingThreshold)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_SleepingThreshold);
            {
                static_cast<const FFieldNode<float>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && ResultsView.Num() > 0)
                    {
                        UpdateMaterialSleepingThreshold(RigidSolver, RigidHandle, ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_DisableThreshold)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_DisableThreshold);
            {
                static_cast<const FFieldNode<float>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
 
                    if (RigidHandle && RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic && ResultsView.Num() > 0)
                    {
                        UpdateMaterialDisableThreshold(RigidSolver, RigidHandle, ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_InternalClusterStrain)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_InternalClusterStrain);
            {
                FRigidClustering& RigidClustering = RigidSolver->GetEvolution()->GetRigidClustering();
                static_cast<const FFieldNode<float>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidClusteredParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToClustered();
                    if (RigidHandle && RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic)
                    {
                        RigidClustering.SetInternalStrain(RigidHandle, RigidHandle->GetInternalStrains() + ResultsView[Index.Result]);
                    }
                }
            }
        }
    }
 
    static void FieldVectorParameterUpdate(Chaos::FPBDRigidsSolver* RigidSolver, const FFieldSystemCommand& FieldCommand,
        TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, 
        Chaos::FPBDPositionConstraints& PositionTarget,
        TMap<int32, int32>& TargetedParticles, TArray<FVector>& FinalResults)
    {
        TFieldArrayView<FVector> ResultsView(FinalResults, 0, FinalResults.Num());
 
        if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearVelocity)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_LinearVelocity);
            {
                static_cast<const FFieldNode<FVector>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic)
                    {
                        RigidHandle->SetV(RigidHandle->GetV() + ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearImpulse)
        {
            SCOPE_CYCLE_COUNTER(STAT_ForceUpdateField_LinearImpulse);
            {
                static_cast<const FFieldNode<FVector>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic)
                    {
                        const FVec3 CurrentImpulseVelocity = RigidHandle->LinearImpulseVelocity();
                        RigidHandle->SetLinearImpulseVelocity(CurrentImpulseVelocity + (ResultsView[Index.Result] * RigidHandle->InvM()));
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_AngularVelociy)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_AngularVelocity);
            {
                static_cast<const FFieldNode<FVector>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
 
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic)
                    {
                        RigidHandle->SetW(RigidHandle->GetW() + ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_PositionTarget)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_PositionTarget);
            {
                static_cast<const FFieldNode<FVector>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidClusteredParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToClustered();
                    if (RigidHandle && ResultsView[Index.Result] != FVector(FLT_MAX))
                    {
                        if (TargetedParticles.Contains(Index.Sample))
                        {
                            const int32 ConstraintIndex = TargetedParticles[Index.Sample];
                            PositionTarget.Replace(ConstraintIndex, ResultsView[Index.Result]);
                        }
                        else
                        {
                            const int32 ConstraintIndex = PositionTarget.NumConstraints();
                            PositionTarget.AddConstraint(RigidHandle, ResultsView[Index.Result]);
                            TargetedParticles.Add(Index.Sample, ConstraintIndex);
                        }
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_PositionAnimated)
        {
            SCOPE_CYCLE_COUNTER(STAT_ParamUpdateField_PositionAnimated);
            {
                UE_LOG(LogChaos, Error, TEXT("Position Animated target currently not supported by chaos"));
            }
        }
    }
 
 
    static void FieldVectorForceUpdate(Chaos::FPBDRigidsSolver* RigidSolver, const FFieldSystemCommand& FieldCommand,
        TArray<Chaos::FGeometryParticleHandle*>& ParticleHandles, FFieldContext& FieldContext, TArray<FVector>& FinalResults)
    {
        TFieldArrayView<FVector> ResultsView(FinalResults, 0, FinalResults.Num());
 
        static_cast<const FFieldNode<FVector>*>(FieldCommand.RootNode.Get())->Evaluate(FieldContext, ResultsView);
 
        if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearForce)
        {
            SCOPE_CYCLE_COUNTER(STAT_ForceUpdateField_LinearForce);
            {
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && !RigidHandle->Disabled() && (RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic || RigidHandle->ObjectState() == Chaos::EObjectStateType::Sleeping))
                    {
                        if (RigidHandle->Sleeping())
                        {
                            RigidHandle->SetObjectStateLowLevel(Chaos::EObjectStateType::Dynamic);
                        }
                        RigidHandle->AddForce(ResultsView[Index.Result]);
                    }
                }
            }
        }
        else if (FieldCommand.PhysicsType == EFieldPhysicsType::Field_AngularTorque)
        {
            SCOPE_CYCLE_COUNTER(STAT_ForceUpdateField_AngularTorque);
            {
                for (const FFieldContextIndex& Index : FieldContext.GetEvaluatedSamples())
                {
                    Chaos::FPBDRigidParticleHandle* RigidHandle = ParticleHandles[Index.Sample]->CastToRigidParticle();
                    if (RigidHandle && (RigidHandle->ObjectState() == Chaos::EObjectStateType::Dynamic || RigidHandle->ObjectState() == Chaos::EObjectStateType::Sleeping))
                    {
                        if (RigidHandle->Sleeping())
                        {
                            RigidHandle->SetObjectStateLowLevel(Chaos::EObjectStateType::Dynamic);
                        }
                        RigidHandle->AddTorque(ResultsView[Index.Result]);
                    }
                }
            }
        }
    }
}
 
FORCEINLINE bool IsParameterFieldValid(const FFieldSystemCommand& FieldCommand)
{
    if (FieldCommand.RootNode->Type() == FFieldNodeBase::EFieldType::EField_Int32)
    {
        return (FieldCommand.PhysicsType == EFieldPhysicsType::Field_DynamicState) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_ActivateDisabled) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_CollisionGroup);
 
    }
    else if (FieldCommand.RootNode->Type() == FFieldNodeBase::EFieldType::EField_Float)
    {
        return (FieldCommand.PhysicsType == EFieldPhysicsType::Field_ExternalClusterStrain) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_Kill) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_SleepingThreshold) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_DisableThreshold) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_InternalClusterStrain);
    }
    else if (FieldCommand.RootNode->Type() == FFieldNodeBase::EFieldType::EField_FVector)
    {
        return (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearVelocity) ||
                (FieldCommand.PhysicsType == EFieldPhysicsType::Field_AngularVelociy) ||
                (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearImpulse);
    }
    return false;
}
 
FORCEINLINE bool IsForceFieldValid(const FFieldSystemCommand& FieldCommand)
{
    if (FieldCommand.RootNode->Type() == FFieldNodeBase::EFieldType::EField_FVector)
    {
        return (FieldCommand.PhysicsType == EFieldPhysicsType::Field_LinearForce) ||
            (FieldCommand.PhysicsType == EFieldPhysicsType::Field_AngularTorque);
    }
    return false;
}