Advertisement
arsenic1234

Untitled

Nov 19th, 2019
113
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.31 KB | None | 0 0
  1. void FAnimNode_SpeedWarping::EvaluateSkeletalControl_AnyThread(FComponentSpacePoseContext & Output, TArray<FBoneTransform>& OutBoneTransforms)
  2. {
  3.     check(OutBoneTransforms.Num() == 0);
  4.     const UWorld * TheWorld = Output.AnimInstanceProxy->GetAnimInstanceObject()->GetWorld();
  5.     const FBoneContainer& BoneContainer = Output.Pose.GetPose().GetBoneContainer();
  6.  
  7.     //get bone index and varify them exists.
  8.     FCompactPoseBoneIndex IKFootRootBoneCompactPoseIndex = IkFootRootBone.GetCompactPoseIndex(BoneContainer);
  9.     FCompactPoseBoneIndex PelvisBoneCompactPoseIndex = PelvisBone.GetCompactPoseIndex(BoneContainer);
  10.     if(IKFootRootBoneCompactPoseIndex == INDEX_NONE || PelvisBoneCompactPoseIndex == INDEX_NONE)
  11.     {
  12.         return;
  13.     }
  14.  
  15.     // Every leg apply a spring force to pelvis.
  16.     TArray<FVector> TSpringForce;
  17.     TArray<FIKFootLocation> TIKFootLocations;
  18.  
  19.     if(!FeetDefinitions.Num() == 0)
  20.     {
  21.         //Calculate each foot's real position.
  22.         for(FIKBones & Each : FeetDefinitions)
  23.         {
  24.             bool bInvalidLimb = false;
  25.             FCompactPoseBoneIndex IKFootBoneCompactPoseIndex = Each.IKFootBone.GetCompactPoseIndex(BoneContainer);
  26.             FCompactPoseBoneIndex FKFootBoneCompactPoseIndex = Each.FKFootBone.GetCompactPoseIndex(BoneContainer);
  27.             FCompactPoseBoneIndex UpperLimbIndex(INDEX_NONE);
  28.             const FCompactPoseBoneIndex LowerLimbIndex = BoneContainer.GetParentBoneIndex(FKFootBoneCompactPoseIndex);
  29.  
  30.             // if any leg is invalid, cancel the whole process.
  31.             if(LowerLimbIndex == INDEX_NONE)
  32.             {
  33.                 bInvalidLimb = true;
  34.             }
  35.             else
  36.             {
  37.                 UpperLimbIndex = BoneContainer.GetParentBoneIndex(LowerLimbIndex);
  38.                 if (UpperLimbIndex == INDEX_NONE)
  39.                 {
  40.                     bInvalidLimb = true;
  41.                 }
  42.             }
  43.             if(bInvalidLimb)
  44.             {
  45.                 return;
  46.             }
  47.  
  48.             // Get Local Space transforms for our bones.
  49.             const FTransform IKBoneLocalTransform = Output.Pose.GetLocalSpaceTransform(IKFootBoneCompactPoseIndex);
  50.             const FTransform EndBoneLocalTransform = Output.Pose.GetLocalSpaceTransform(FKFootBoneCompactPoseIndex);
  51.             const FTransform LowerLimbLocalTransform = Output.Pose.GetLocalSpaceTransform(LowerLimbIndex);
  52.             const FTransform UpperLimbLocalTransform = Output.Pose.GetLocalSpaceTransform(UpperLimbIndex);
  53.  
  54.             // Get Component Space transforms for our bones.
  55.             FTransform IKBoneCSTransform = Output.Pose.GetComponentSpaceTransform(IKFootBoneCompactPoseIndex);
  56.             FTransform EndBoneCSTransform = Output.Pose.GetComponentSpaceTransform(FKFootBoneCompactPoseIndex);
  57.             FTransform LowerLimbCSTransform = Output.Pose.GetComponentSpaceTransform(LowerLimbIndex);
  58.             FTransform UpperLimbCSTransform = Output.Pose.GetComponentSpaceTransform(UpperLimbIndex);
  59.  
  60.             // Get current position of root of limb.
  61.             // All position are in Component space.
  62.             const FVector RootPos = UpperLimbCSTransform.GetTranslation();
  63.             const FVector InitialJointPos = LowerLimbCSTransform.GetTranslation();
  64.             const FVector InitialEndPos = EndBoneCSTransform.GetTranslation();
  65.             const FVector OriginIKPos = IKBoneCSTransform.GetTranslation();
  66.  
  67.             // Length of limbs.
  68.             float LowerLimbLength = (EndBoneCSTransform.GetLocation() - LowerLimbCSTransform.GetLocation()).Size();
  69.             float UpperLimbLength = (LowerLimbCSTransform.GetLocation() - UpperLimbCSTransform.GetLocation()).Size();
  70.  
  71.             // Get scale pivot
  72.             const FVector ScalePivot = FVector(RootPos.X, RootPos.Y, OriginIKPos.Z);
  73.  
  74.             // Calculate new IKPos
  75.             FVector NewIKPos = OriginIKPos;
  76.             switch (SpeedWarpingAxisMode)
  77.             {
  78.             case EIKFootRootLocalAxis::NONE:
  79.                 break;
  80.             case EIKFootRootLocalAxis::X:
  81.                 NewIKPos.X = ((OriginIKPos - ScalePivot).X * SpeedScaling) + ScalePivot.X;
  82.             case EIKFootRootLocalAxis::Y:
  83.                 NewIKPos.Y = ((OriginIKPos - ScalePivot).Y * SpeedScaling) + ScalePivot.Y;
  84.             case EIKFootRootLocalAxis::Z:
  85.                 NewIKPos.Z = ((OriginIKPos - ScalePivot).Z * SpeedScaling) + ScalePivot.Z;
  86.             default:
  87.                 break;
  88.             }
  89.  
  90.             // Clamp Ik foot position use total leg length.
  91.             FVector ActualIKPos = NewIKPos;
  92.             if(ClampIKUsingFKLeg)
  93.             {
  94.                 float OriginIKLength = (OriginIKPos - RootPos).Size();
  95.                 float NewIkLength = (NewIKPos - RootPos).Size();
  96.                 float CutoffPercent = 1 - (1 / SpeedScaling);
  97.                 float TotalLimbLength = LowerLimbLength + UpperLimbLength;
  98.                 float MaxScaledLimbLength = OriginIKLength + (TotalLimbLength - OriginIKLength) * CutoffPercent;
  99.                 FVector IKDirection = (NewIKPos - RootPos).GetSafeNormal();
  100.                 if (NewIkLength > OriginIKLength)
  101.                 {
  102.                     FVector AdjustedIKPos = NewIKPos + IKDirection * (MaxScaledLimbLength - NewIkLength);
  103.                     ActualIKPos = (AdjustedIKPos - RootPos).Size() > (ActualIKPos - RootPos).Size() ? ActualIKPos : AdjustedIKPos;
  104.                 }
  105.             }
  106.  
  107.             // Set Ik foot's actual position.
  108.             IKBoneCSTransform.SetTranslation(ActualIKPos);
  109.             OutBoneTransforms.Add(FBoneTransform(IKFootBoneCompactPoseIndex, IKBoneCSTransform));
  110.  
  111.             // Record origin and actual IK foot position.
  112.             FIKFootLocation IKFootLocation;
  113.             IKFootLocation.LimbRootLocation = RootPos;
  114.             IKFootLocation.OriginLocation = OriginIKPos;
  115.             IKFootLocation.ActualLocation = ActualIKPos;
  116.             IKFootLocation.NewLocation = NewIKPos;
  117.             TIKFootLocations.Add(IKFootLocation);
  118.         }
  119.     }
  120.  
  121.     // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  122.     // Calculate influence upon pelvis by this leg, according to actual Ik foot position.
  123.     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  124.     for(FIKFootLocation EachIK : TIKFootLocations)
  125.     {
  126.         // Init values first time
  127.         RemainingTime = DeltaTime;
  128.         BoneLocation = EachIK.OriginLocation;
  129.         BoneVelocity = FVector::ZeroVector;
  130.         //      GEngine->AddOnScreenDebugMessage(17, 10, FColor::Green, FString::Printf(TEXT("OriginIKSize: %0.5f"), EachIK.OriginLocation.Size()));
  131.  
  132.         while(RemainingTime > TimeStep)
  133.         {
  134.             // Calculate error vector.
  135.             FVector const IKDirection = (EachIK.ActualLocation - EachIK.LimbRootLocation).GetSafeNormal();
  136.             float IKStretch = ((EachIK.ActualLocation - EachIK.LimbRootLocation).Size() - (EachIK.OriginLocation - EachIK.LimbRootLocation).Size());
  137.             FVector const Error = IKDirection * IKStretch;
  138.             FVector const SpringForce = PelvisAdjustmentInterp.Stiffness * Error;
  139.             FVector const DampingForce = PelvisAdjustmentInterp.Dampen * BoneVelocity;
  140.  
  141.             // Calculate force based on error and velocity
  142.             FVector const Acceleration = SpringForce - DampingForce;
  143.  
  144.             // Integrate velocity
  145.             // Make sure damping with variable frame rate actually dampens velocity. Otherwise Spring will go nuts.
  146.             float const CutOffDampingValue = 1.f / TimeStep;
  147.             if (PelvisAdjustmentInterp.Dampen > CutOffDampingValue)
  148.             {
  149.                 float const SafetyScale = CutOffDampingValue / PelvisAdjustmentInterp.Dampen;
  150.                 BoneVelocity += SafetyScale * (Acceleration * TimeStep);
  151.             }
  152.             else
  153.             {
  154.                 BoneVelocity += (Acceleration * TimeStep);
  155.             }
  156.  
  157.             // Integrate position
  158.             FVector const OldBoneLocation = BoneLocation;
  159.             FVector const DeltaMove = (BoneVelocity * TimeStep);
  160.             BoneLocation += DeltaMove;
  161.  
  162.             // Update velocity to reflect post processing done to bone location.
  163.             BoneVelocity = (BoneLocation - OldBoneLocation) / TimeStep;
  164.  
  165.             check(!BoneLocation.ContainsNaN());
  166.             check(!BoneVelocity.ContainsNaN());
  167.  
  168.             // End of calculation.
  169.             RemainingTime -= TimeStep;
  170.         }
  171.         TSpringForce.Add(BoneVelocity);
  172.     }
  173.     // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  174.  
  175.     // Calculate resultant force.
  176.     FVector ResultantForce = FVector(0.0f, 0.0f, 0.0f);
  177.     for(FVector const EachVector : TSpringForce)
  178.     {
  179.         ResultantForce += EachVector;
  180.     }
  181.  
  182.     //Calculate actual adjusted pelvis posiiton.
  183.     FTransform PelvisBoneCSTransform = Output.Pose.GetComponentSpaceTransform(PelvisBoneCompactPoseIndex);
  184.     FVector OriginPelvisLocation = PelvisBoneCSTransform.GetLocation();
  185.     FVector NewPelvisLocation = OriginPelvisLocation + ResultantForce;
  186.     FVector ActralPelvisLocation = OriginPelvisLocation + (ResultantForce * PelvisAdjustmentAlpha);
  187.  
  188.     // Set new pelvis transform.
  189.     PelvisBoneCSTransform.SetTranslation(ActralPelvisLocation);
  190.     OutBoneTransforms.Insert(FBoneTransform(PelvisBoneCompactPoseIndex, PelvisBoneCSTransform), 0);
  191.  
  192. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement