void FAnimNode_RotationMultiplier::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer & RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
if ( Multiplier != 0.f )
{
// Reference bone
const TArray<FTransform> & LocalRefPose = RequiredBones.GetRefPoseArray();
const FQuat RefQuat = LocalRefPose[TargetBone.BoneIndex].GetRotation();
FQuat NewQuat;
MultiplyQuatBasedOnSourceIndex(LocalRefPose, MeshBases, RotationAxisToRefer, SourceBone.BoneIndex, Multiplier, RefQuat, NewQuat);
FTransform NewLocalTransform = MeshBases.GetLocalSpaceTransform(TargetBone.BoneIndex);
NewLocalTransform.SetRotation(NewQuat);
const int32 ParentIndex = RequiredBones.GetParentBoneIndex(TargetBone.BoneIndex);
if( ParentIndex != INDEX_NONE )
{
const FTransform ParentTM = MeshBases.GetComponentSpaceTransform(ParentIndex);
FTransform NewTransform = NewLocalTransform * ParentTM;
OutBoneTransforms.Add( FBoneTransform(TargetBone.BoneIndex, NewTransform) );
}
else
{
OutBoneTransforms.Add( FBoneTransform(TargetBone.BoneIndex, NewLocalTransform) );
}
}
}
void FAnimNode_WheelHandler::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
for(const auto & WheelSim : WheelSimulators)
{
if(WheelSim.BoneReference.IsValid(RequiredBones))
{
// the way we apply transform is same as FMatrix or FTransform
// we apply scale first, and rotation, and translation
// if you'd like to translate first, you'll need two nodes that first node does translate and second nodes to rotate.
FTransform NewBoneTM = MeshBases.GetComponentSpaceTransform(WheelSim.BoneReference.BoneIndex);
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, WheelSim.BoneReference.BoneIndex, BCS_ComponentSpace);
// Apply rotation offset
const FQuat BoneQuat(WheelSim.RotOffset);
NewBoneTM.SetRotation(BoneQuat * NewBoneTM.GetRotation());
// Apply loc offset
NewBoneTM.AddToTranslation(WheelSim.LocOffset);
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, WheelSim.BoneReference.BoneIndex, BCS_ComponentSpace);
// add back to it
OutBoneTransforms.Add( FBoneTransform(WheelSim.BoneReference.BoneIndex, NewBoneTM) );
}
}
}
void FAnimNode_CopyBone::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer & RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
// Pass through if we're not doing anything.
if( !bCopyTranslation && !bCopyRotation && !bCopyScale )
{
return;
}
// Get component space transform for source and current bone.
FTransform SourceBoneTM = MeshBases.GetComponentSpaceTransform(SourceBone.BoneIndex);
FTransform CurrentBoneTM = MeshBases.GetComponentSpaceTransform(TargetBone.BoneIndex);
// Copy individual components
if (bCopyTranslation)
{
CurrentBoneTM.SetTranslation( SourceBoneTM.GetTranslation() );
}
if (bCopyRotation)
{
CurrentBoneTM.SetRotation( SourceBoneTM.GetRotation() );
}
if (bCopyScale)
{
CurrentBoneTM.SetScale3D( SourceBoneTM.GetScale3D() );
}
// Output new transform for current bone.
OutBoneTransforms.Add( FBoneTransform(TargetBone.BoneIndex, CurrentBoneTM) );
}
void FAnimNode_HandIKRetargeting::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
checkSlow(OutBoneTransforms.Num() == 0);
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
// Get component space transforms for all of our IK and FK bones.
const FTransform& RightHandFKTM = MeshBases.GetComponentSpaceTransform(RightHandFK.GetCompactPoseIndex(BoneContainer));
const FTransform& LeftHandFKTM = MeshBases.GetComponentSpaceTransform(LeftHandFK.GetCompactPoseIndex(BoneContainer));
const FTransform& RightHandIKTM = MeshBases.GetComponentSpaceTransform(RightHandIK.GetCompactPoseIndex(BoneContainer));
const FTransform& LeftHandIKTM = MeshBases.GetComponentSpaceTransform(LeftHandIK.GetCompactPoseIndex(BoneContainer));
// Compute weight FK and IK hand location. And translation from IK to FK.
FVector const FKLocation = FMath::Lerp<FVector>(LeftHandFKTM.GetTranslation(), RightHandFKTM.GetTranslation(), HandFKWeight);
FVector const IKLocation = FMath::Lerp<FVector>(LeftHandIKTM.GetTranslation(), RightHandIKTM.GetTranslation(), HandFKWeight);
FVector const IK_To_FK_Translation = FKLocation - IKLocation;
// If we're not translating, don't send any bones to update.
if (!IK_To_FK_Translation.IsNearlyZero())
{
// Move desired bones
for (const FBoneReference& BoneReference : IKBonesToMove)
{
if (BoneReference.IsValid(BoneContainer))
{
FCompactPoseBoneIndex BoneIndex = BoneReference.GetCompactPoseIndex(BoneContainer);
FTransform BoneTransform = MeshBases.GetComponentSpaceTransform(BoneIndex);
BoneTransform.AddToTranslation(IK_To_FK_Translation);
OutBoneTransforms.Add(FBoneTransform(BoneIndex, BoneTransform));
}
}
}
}
void FAnimNode_HandIKRetargeting::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
checkSlow(OutBoneTransforms.Num() == 0);
// Get component space transforms for all of our IK and FK bones.
FTransform const RightHandFKTM = MeshBases.GetComponentSpaceTransform(RightHandFK.BoneIndex);
FTransform const LeftHandFKTM = MeshBases.GetComponentSpaceTransform(LeftHandFK.BoneIndex);
FTransform const RightHandIKTM = MeshBases.GetComponentSpaceTransform(RightHandIK.BoneIndex);
FTransform const LeftHandIKTM = MeshBases.GetComponentSpaceTransform(LeftHandIK.BoneIndex);
// Compute weight FK and IK hand location. And translation from IK to FK.
FVector const FKLocation = FMath::Lerp<FVector>(LeftHandFKTM.GetTranslation(), RightHandFKTM.GetTranslation(), HandFKWeight);
FVector const IKLocation = FMath::Lerp<FVector>(LeftHandIKTM.GetTranslation(), RightHandIKTM.GetTranslation(), HandFKWeight);
FVector const IK_To_FK_Translation = FKLocation - IKLocation;
// If we're not translating, don't send any bones to update.
if (!IK_To_FK_Translation.IsNearlyZero())
{
// Move desired bones
for (int32 BoneIndex = 0; BoneIndex < IKBonesToMove.Num(); BoneIndex++)
{
FBoneReference const & BoneReference = IKBonesToMove[BoneIndex];
if (BoneReference.IsValid(RequiredBones))
{
FTransform BoneTransform = MeshBases.GetComponentSpaceTransform(BoneReference.BoneIndex);
BoneTransform.AddToTranslation(IK_To_FK_Translation);
OutBoneTransforms.Add(FBoneTransform(BoneReference.BoneIndex, BoneTransform));
}
}
}
}
void FAnimNode_WheelHandler::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
const TArray<FWheelAnimData>& WheelAnimData = AnimInstanceProxy->GetWheelAnimData();
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
for(const FWheelLookupData& Wheel : Wheels)
{
if (Wheel.BoneReference.IsValid(BoneContainer))
{
FCompactPoseBoneIndex WheelSimBoneIndex = Wheel.BoneReference.GetCompactPoseIndex(BoneContainer);
// the way we apply transform is same as FMatrix or FTransform
// we apply scale first, and rotation, and translation
// if you'd like to translate first, you'll need two nodes that first node does translate and second nodes to rotate.
FTransform NewBoneTM = MeshBases.GetComponentSpaceTransform(WheelSimBoneIndex);
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, WheelSimBoneIndex, BCS_ComponentSpace);
// Apply rotation offset
const FQuat BoneQuat(WheelAnimData[Wheel.WheelIndex].RotOffset);
NewBoneTM.SetRotation(BoneQuat * NewBoneTM.GetRotation());
// Apply loc offset
NewBoneTM.AddToTranslation(WheelAnimData[Wheel.WheelIndex].LocOffset);
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, WheelSimBoneIndex, BCS_ComponentSpace);
// add back to it
OutBoneTransforms.Add(FBoneTransform(WheelSimBoneIndex, NewBoneTM));
}
}
}
void FAnimNode_CopyBone::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
// Pass through if we're not doing anything.
if( !bCopyTranslation && !bCopyRotation && !bCopyScale )
{
return;
}
// Get component space transform for source and current bone.
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
FCompactPoseBoneIndex SourceBoneIndex = SourceBone.GetCompactPoseIndex(BoneContainer);
FCompactPoseBoneIndex TargetBoneIndex = TargetBone.GetCompactPoseIndex(BoneContainer);
FTransform SourceBoneTM = MeshBases.GetComponentSpaceTransform(SourceBoneIndex);
FTransform CurrentBoneTM = MeshBases.GetComponentSpaceTransform(TargetBoneIndex);
if(ControlSpace != BCS_ComponentSpace)
{
// Convert out to selected space
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, SourceBoneTM, SourceBoneIndex, ControlSpace);
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, CurrentBoneTM, TargetBoneIndex, ControlSpace);
}
// Copy individual components
if (bCopyTranslation)
{
CurrentBoneTM.SetTranslation( SourceBoneTM.GetTranslation() );
}
if (bCopyRotation)
{
CurrentBoneTM.SetRotation( SourceBoneTM.GetRotation() );
}
if (bCopyScale)
{
CurrentBoneTM.SetScale3D( SourceBoneTM.GetScale3D() );
}
if(ControlSpace != BCS_ComponentSpace)
{
// Convert back out if we aren't operating in component space
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, CurrentBoneTM, TargetBoneIndex, ControlSpace);
}
// Output new transform for current bone.
OutBoneTransforms.Add(FBoneTransform(TargetBoneIndex, CurrentBoneTM));
}
void FAnimNode_RotationMultiplier::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
if ( Multiplier != 0.f )
{
// Reference bone
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
const FCompactPoseBoneIndex TargetBoneIndex = TargetBone.GetCompactPoseIndex(BoneContainer);
const FCompactPoseBoneIndex SourceBoneIndex = SourceBone.GetCompactPoseIndex(BoneContainer);
const FQuat RefQuat = MeshBases.GetPose().GetRefPose(TargetBoneIndex).GetRotation();
const FTransform& SourceRefPose = MeshBases.GetPose().GetRefPose(SourceBoneIndex);
FQuat NewQuat = MultiplyQuatBasedOnSourceIndex(SourceRefPose, MeshBases.GetLocalSpaceTransform(SourceBoneIndex), RotationAxisToRefer, Multiplier, RefQuat);
FTransform NewLocalTransform = MeshBases.GetLocalSpaceTransform(TargetBoneIndex);
if (bIsAdditive)
{
NewQuat = NewLocalTransform.GetRotation() * NewQuat;
}
NewLocalTransform.SetRotation(NewQuat);
const FCompactPoseBoneIndex ParentIndex = MeshBases.GetPose().GetParentBoneIndex(TargetBoneIndex);
if( ParentIndex != INDEX_NONE )
{
const FTransform& ParentTM = MeshBases.GetComponentSpaceTransform(ParentIndex);
FTransform NewTransform = NewLocalTransform * ParentTM;
OutBoneTransforms.Add( FBoneTransform(TargetBoneIndex, NewTransform) );
}
else
{
OutBoneTransforms.Add( FBoneTransform(TargetBoneIndex, NewLocalTransform) );
}
}
}
void FAnimNode_LookAt::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
FTransform ComponentBoneTransform = MeshBases.GetComponentSpaceTransform(BoneToModify.BoneIndex);
// get target location
FVector TargetLocationInComponentSpace;
if (LookAtBone.IsValid(RequiredBones))
{
FTransform LookAtTransform = MeshBases.GetComponentSpaceTransform(LookAtBone.BoneIndex);
TargetLocationInComponentSpace = LookAtTransform.GetLocation();
}
else
{
TargetLocationInComponentSpace = SkelComp->ComponentToWorld.InverseTransformPosition(LookAtLocation);
}
CurrentLookAtLocation = TargetLocationInComponentSpace;
// lookat vector
FVector LookAtVector = GetAlignVector(ComponentBoneTransform, LookAtAxis);
// flip to target vector if it wasnt negative
bool bShouldFlip = LookAtAxis == EAxisOption::X_Neg || LookAtAxis == EAxisOption::Y_Neg || LookAtAxis == EAxisOption::Z_Neg;
FVector ToTarget = CurrentLookAtLocation - ComponentBoneTransform.GetLocation();
ToTarget.Normalize();
if (bShouldFlip)
{
ToTarget *= -1.f;
}
// get delta rotation
FQuat DeltaRot = FQuat::FindBetween(LookAtVector, ToTarget);
// transform current rotation to delta rotation
FQuat CurrentRot = ComponentBoneTransform.GetRotation();
FQuat NewRotation = DeltaRot * CurrentRot;
ComponentBoneTransform.SetRotation(NewRotation);
OutBoneTransforms.Add( FBoneTransform(BoneToModify.BoneIndex, ComponentBoneTransform) );
}
void FAnimNode_CopyBoneDelta::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
if(!bCopyTranslation && !bCopyRotation && !bCopyScale)
{
return;
}
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
FCompactPoseBoneIndex SourceBoneIndex = SourceBone.GetCompactPoseIndex(BoneContainer);
FCompactPoseBoneIndex TargetBoneIndex = TargetBone.GetCompactPoseIndex(BoneContainer);
FTransform SourceTM = MeshBases.GetComponentSpaceTransform(SourceBoneIndex);
FTransform TargetTM = MeshBases.GetComponentSpaceTransform(TargetBoneIndex);
// Convert to parent space
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, SourceTM, SourceBoneIndex, BCS_ParentBoneSpace);
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, TargetTM, TargetBoneIndex, BCS_ParentBoneSpace);
// Ref pose transform
FTransform RefLSTransform = SkelComp->SkeletalMesh->RefSkeleton.GetRefBonePose()[SourceBone.GetMeshPoseIndex().GetInt()];
// Get transform relative to ref pose
SourceTM.SetToRelativeTransform(RefLSTransform);
if(CopyMode == CopyBoneDeltaMode::Accumulate)
{
if(bCopyTranslation)
{
TargetTM.AddToTranslation(SourceTM.GetTranslation() * TranslationMultiplier);
}
if(bCopyRotation)
{
FVector Axis;
float Angle;
SourceTM.GetRotation().ToAxisAndAngle(Axis, Angle);
TargetTM.SetRotation(FQuat(Axis, Angle * RotationMultiplier) * TargetTM.GetRotation());
}
if(bCopyScale)
{
TargetTM.SetScale3D(TargetTM.GetScale3D() * (SourceTM.GetScale3D() * ScaleMultiplier));
}
}
else //CopyMode = CopyBoneDeltaMode::Copy
{
if(bCopyTranslation)
{
TargetTM.SetTranslation(SourceTM.GetTranslation() * TranslationMultiplier);
}
if(bCopyRotation)
{
FVector Axis;
float Angle;
SourceTM.GetRotation().ToAxisAndAngle(Axis, Angle);
TargetTM.SetRotation(FQuat(Axis, Angle * RotationMultiplier));
}
if(bCopyScale)
{
TargetTM.SetScale3D(SourceTM.GetScale3D() * ScaleMultiplier);
}
}
// Back out to component space
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, TargetTM, TargetBoneIndex, BCS_ParentBoneSpace);
OutBoneTransforms.Add(FBoneTransform(TargetBoneIndex, TargetTM));
}
void FAnimNode_Trail::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
if( ChainLength < 2 )
{
return;
}
// The incoming BoneIndex is the 'end' of the spline chain. We need to find the 'start' by walking SplineLength bones up hierarchy.
// Fail if we walk past the root bone.
int32 WalkBoneIndex = TrailBone.BoneIndex;
TArray<int32> ChainBoneIndices;
ChainBoneIndices.AddZeroed(ChainLength);
ChainBoneIndices[ChainLength - 1] = WalkBoneIndex;
for (int32 i = 1; i < ChainLength; i++)
{
// returns to avoid a crash
// @TODO : shows an error message why failed
if (WalkBoneIndex == 0)
{
return;
}
// Get parent bone.
WalkBoneIndex = RequiredBones.GetParentBoneIndex(WalkBoneIndex);
//Insert indices at the start of array, so that parents are before children in the array.
int32 TransformIndex = ChainLength - (i + 1);
ChainBoneIndices[TransformIndex] = WalkBoneIndex;
}
OutBoneTransforms.AddZeroed(ChainLength);
// If we have >0 this frame, but didn't last time, record positions of all the bones.
// Also do this if number has changed or array is zero.
bool bHasValidStrength = (Alpha > 0.f);
if(TrailBoneLocations.Num() != ChainLength || (bHasValidStrength && !bHadValidStrength))
{
TrailBoneLocations.Empty();
TrailBoneLocations.AddZeroed(ChainLength);
for(int32 i=0; i<ChainBoneIndices.Num(); i++)
{
int32 ChildIndex = ChainBoneIndices[i];
FTransform ChainTransform = MeshBases.GetComponentSpaceTransform(ChildIndex);
TrailBoneLocations[i] = ChainTransform.GetTranslation();
}
OldLocalToWorld = SkelComp->GetTransformMatrix();
}
bHadValidStrength = bHasValidStrength;
// transform between last frame and now.
FMatrix OldToNewTM = OldLocalToWorld * SkelComp->GetTransformMatrix().InverseFast();
// Add fake velocity if present to all but root bone
if(!FakeVelocity.IsZero())
{
FVector FakeMovement = -FakeVelocity * ThisTimstep;
if (bActorSpaceFakeVel && SkelComp->GetOwner())
{
const FTransform BoneToWorld(SkelComp->GetOwner()->GetActorRotation(), SkelComp->GetOwner()->GetActorLocation());
FakeMovement = BoneToWorld.TransformVector(FakeMovement);
}
FakeMovement = SkelComp->GetTransformMatrix().InverseTransformVector(FakeMovement);
// Then add to each bone
for(int32 i=1; i<TrailBoneLocations.Num(); i++)
{
TrailBoneLocations[i] += FakeMovement;
}
}
// Root bone of trail is not modified.
int32 RootIndex = ChainBoneIndices[0];
FTransform ChainTransform = MeshBases.GetComponentSpaceTransform(RootIndex);
OutBoneTransforms[0] = FBoneTransform(RootIndex, ChainTransform);
TrailBoneLocations[0] = ChainTransform.GetTranslation();
// Starting one below head of chain, move bones.
for(int32 i=1; i<ChainBoneIndices.Num(); i++)
{
// Parent bone position in component space.
int32 ParentIndex = ChainBoneIndices[i-1];
FVector ParentPos = TrailBoneLocations[i-1];
FVector ParentAnimPos = MeshBases.GetComponentSpaceTransform(ParentIndex).GetTranslation();
// Child bone position in component space.
int32 ChildIndex = ChainBoneIndices[i];
FVector ChildPos = OldToNewTM.TransformPosition(TrailBoneLocations[i]); // move from 'last frames component' frame to 'this frames component' frame
FVector ChildAnimPos = MeshBases.GetComponentSpaceTransform(ChildIndex).GetTranslation();
// Desired parent->child offset.
FVector TargetDelta = (ChildAnimPos - ParentAnimPos);
// Desired child position.
FVector ChildTarget = ParentPos + TargetDelta;
// Find vector from child to target
FVector Error = ChildTarget - ChildPos;
// Calculate how much to push the child towards its target
float Correction = FMath::Clamp<float>(ThisTimstep * TrailRelaxation, 0.f, 1.f);
// Scale correction vector and apply to get new world-space child position.
TrailBoneLocations[i] = ChildPos + (Error * Correction);
// If desired, prevent bones stretching too far.
if(bLimitStretch)
{
float RefPoseLength = TargetDelta.Size();
FVector CurrentDelta = TrailBoneLocations[i] - TrailBoneLocations[i-1];
float CurrentLength = CurrentDelta.Size();
// If we are too far - cut it back (just project towards parent particle).
if( (CurrentLength - RefPoseLength > StretchLimit) && CurrentLength > SMALL_NUMBER )
{
FVector CurrentDir = CurrentDelta / CurrentLength;
TrailBoneLocations[i] = TrailBoneLocations[i-1] + (CurrentDir * (RefPoseLength + StretchLimit));
}
}
// Modify child matrix
OutBoneTransforms[i] = FBoneTransform(ChildIndex, MeshBases.GetComponentSpaceTransform(ChildIndex));
OutBoneTransforms[i].Transform.SetTranslation(TrailBoneLocations[i]);
// Modify rotation of parent matrix to point at this one.
// Calculate the direction that parent bone is currently pointing.
FVector CurrentBoneDir = OutBoneTransforms[i-1].Transform.TransformVector( GetAlignVector(ChainBoneAxis, bInvertChainBoneAxis) );
CurrentBoneDir = CurrentBoneDir.SafeNormal(SMALL_NUMBER);
// Calculate vector from parent to child.
FVector NewBoneDir = FVector(OutBoneTransforms[i].Transform.GetTranslation() - OutBoneTransforms[i - 1].Transform.GetTranslation()).SafeNormal(SMALL_NUMBER);
// Calculate a quaternion that gets us from our current rotation to the desired one.
FQuat DeltaLookQuat = FQuat::FindBetween(CurrentBoneDir, NewBoneDir);
FTransform DeltaTM( DeltaLookQuat, FVector(0.f) );
// Apply to the current parent bone transform.
FTransform TmpMatrix = FTransform::Identity;
TmpMatrix.CopyRotationPart(OutBoneTransforms[i - 1].Transform);
TmpMatrix = TmpMatrix * DeltaTM;
OutBoneTransforms[i - 1].Transform.CopyRotationPart(TmpMatrix);
}
// For the last bone in the chain, use the rotation from the bone above it.
OutBoneTransforms[ChainLength - 1].Transform.CopyRotationPart(OutBoneTransforms[ChainLength - 2].Transform);
// Update OldLocalToWorld
OldLocalToWorld = SkelComp->GetTransformMatrix();
}
void FAnimNode_LookAt::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
const FCompactPoseBoneIndex ModifyBoneIndex = BoneToModify.GetCompactPoseIndex(BoneContainer);
FTransform ComponentBoneTransform = MeshBases.GetComponentSpaceTransform(ModifyBoneIndex);
// get target location
FVector TargetLocationInComponentSpace;
if (LookAtBone.IsValid(BoneContainer))
{
const FTransform& LookAtTransform = MeshBases.GetComponentSpaceTransform(LookAtBone.GetCompactPoseIndex(BoneContainer));
TargetLocationInComponentSpace = LookAtTransform.GetLocation();
}
else
{
TargetLocationInComponentSpace = SkelComp->ComponentToWorld.InverseTransformPosition(LookAtLocation);
}
FVector OldCurrentTargetLocation = CurrentTargetLocation;
FVector NewCurrentTargetLocation = TargetLocationInComponentSpace;
if ((NewCurrentTargetLocation - OldCurrentTargetLocation).SizeSquared() > InterpolationTriggerThreashold*InterpolationTriggerThreashold)
{
if (AccumulatedInterpoolationTime >= InterpolationTime)
{
// reset current Alpha, we're starting to move
AccumulatedInterpoolationTime = 0.f;
}
PreviousTargetLocation = OldCurrentTargetLocation;
CurrentTargetLocation = NewCurrentTargetLocation;
}
else if (InterpolationTriggerThreashold == 0.f)
{
CurrentTargetLocation = NewCurrentTargetLocation;
}
if (InterpolationTime > 0.f)
{
float CurrentAlpha = AccumulatedInterpoolationTime/InterpolationTime;
if (CurrentAlpha < 1.f)
{
float BlendAlpha = AlphaToBlendType(CurrentAlpha, GetInterpolationType());
CurrentLookAtLocation = FMath::Lerp(PreviousTargetLocation, CurrentTargetLocation, BlendAlpha);
}
}
else
{
CurrentLookAtLocation = CurrentTargetLocation;
}
if (bEnableDebug)
{
UWorld* World = SkelComp->GetWorld();
DrawDebugData(World, SkelComp->GetComponentToWorld(), ComponentBoneTransform.GetLocation(), PreviousTargetLocation, FColor(0, 255, 0));
DrawDebugData(World, SkelComp->GetComponentToWorld(), ComponentBoneTransform.GetLocation(), CurrentTargetLocation, FColor(255, 0, 0));
DrawDebugData(World, SkelComp->GetComponentToWorld(), ComponentBoneTransform.GetLocation(), CurrentLookAtLocation, FColor(0, 0, 255));
}
// lookat vector
FVector LookAtVector = GetAlignVector(ComponentBoneTransform, LookAtAxis);
// flip to target vector if it wasnt negative
bool bShouldFlip = LookAtAxis == EAxisOption::X_Neg || LookAtAxis == EAxisOption::Y_Neg || LookAtAxis == EAxisOption::Z_Neg;
FVector ToTarget = CurrentLookAtLocation - ComponentBoneTransform.GetLocation();
ToTarget.Normalize();
if (bShouldFlip)
{
ToTarget *= -1.f;
}
if ( LookAtClamp > ZERO_ANIMWEIGHT_THRESH )
{
float LookAtClampInRadians = FMath::DegreesToRadians(LookAtClamp);
float DiffAngle = FMath::Acos(FVector::DotProduct(LookAtVector, ToTarget));
if (LookAtClampInRadians > 0.f && DiffAngle > LookAtClampInRadians)
{
FVector OldToTarget = ToTarget;
FVector DeltaTarget = ToTarget-LookAtVector;
float Ratio = LookAtClampInRadians/DiffAngle;
DeltaTarget *= Ratio;
ToTarget = LookAtVector + DeltaTarget;
ToTarget.Normalize();
// UE_LOG(LogAnimation, Warning, TEXT("Recalculation required - old target %f, new target %f"),
// FMath::RadiansToDegrees(FMath::Acos(FVector::DotProduct(LookAtVector, OldToTarget))), FMath::RadiansToDegrees(FMath::Acos(FVector::DotProduct(LookAtVector, ToTarget))));
}
}
FQuat DeltaRot;
// if want to use look up,
if (bUseLookUpAxis)
{
// find look up vector in local space
FVector LookUpVector = GetAlignVector(ComponentBoneTransform, LookUpAxis);
// project target to the plane
FVector NewTarget = FVector::VectorPlaneProject(ToTarget, LookUpVector);
NewTarget.Normalize();
DeltaRot = FQuat::FindBetween(LookAtVector, NewTarget);
}
else
{
DeltaRot = FQuat::FindBetween(LookAtVector, ToTarget);
}
// transform current rotation to delta rotation
FQuat CurrentRot = ComponentBoneTransform.GetRotation();
FQuat NewRotation = DeltaRot * CurrentRot;
ComponentBoneTransform.SetRotation(NewRotation);
OutBoneTransforms.Add(FBoneTransform(ModifyBoneIndex, ComponentBoneTransform));
}
void FAnimNode_KinectV2Retarget::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
uint8 i = 0;
if (!KinectBody.bIsTracked)
{
return;
}
const FBoneContainer BoneContainer = MeshBases.GetPose().GetBoneContainer();
FA2CSPose TempPose;
TempPose.AllocateLocalPoses(BoneContainer, SkelComp->LocalAtoms);
for (auto Bone : KinectBody.KinectBones)
{
if (BonesToRetarget[i].IsValid(BoneContainer))
{
auto DeltaTranform = Bone.MirroredJointTransform.GetRelativeTransform(SkelComp->GetBoneTransform(0));
//AxisMeshes[Bone.JointTypeEnd]->SetRelativeLocation(PosableMesh->GetBoneLocationByName(RetargetBoneNames[Bone.JointTypeEnd], EBoneSpaces::ComponentSpace));
auto BoneBaseTransform = DeltaTranform*SkelComp->GetBoneTransform(0);
FRotator PreAdjusmentRotator = BoneBaseTransform.Rotator();
FRotator PostBoneDirAdjustmentRotator = (BoneAdjustments[Bone.JointTypeEnd].BoneDirAdjustment.Quaternion()*PreAdjusmentRotator.Quaternion()).Rotator();
FRotator CompSpaceRotator = (PostBoneDirAdjustmentRotator.Quaternion()*BoneAdjustments[Bone.JointTypeEnd].BoneNormalAdjustment.Quaternion()).Rotator();
FVector Normal, Binormal, Dir;
UKismetMathLibrary::BreakRotIntoAxes(CompSpaceRotator, Normal, Binormal, Dir);
Dir *= BoneAdjustments[Bone.JointTypeEnd].bInvertDir ? -1 : 1;
Normal *= BoneAdjustments[Bone.JointTypeEnd].bInvertNormal ? -1 : 1;
FVector X, Y, Z;
switch (BoneAdjustments[Bone.JointTypeEnd].BoneDirAxis)
{
case EAxis::X:
X = Dir;
break;
case EAxis::Y:
Y = Dir;
break;
case EAxis::Z:
Z = Dir;
break;
default:
;
}
switch (BoneAdjustments[Bone.JointTypeEnd].BoneBinormalAxis)
{
case EAxis::X:
X = Binormal;
break;
case EAxis::Y:
Y = Binormal;
break;
case EAxis::Z:
Z = Binormal;
break;
default:
;
}
switch (BoneAdjustments[Bone.JointTypeEnd].BoneNormalAxis)
{
case EAxis::X:
X = Normal;
break;
case EAxis::Y:
Y = Normal;
break;
case EAxis::Z:
Z = Normal;
break;
default:
;
}
FRotator SwiveledRot = UKismetMathLibrary::MakeRotationFromAxes(X, Y, Z);
SwiveledRot = (SkelComp->GetBoneTransform(0).Rotator().Quaternion()*SwiveledRot.Quaternion()).Rotator();
//PosableMesh->SetBoneRotationByName(RetargetBoneNames[Bone.JointTypeEnd], (PosableMesh->GetBoneTransform(0).Rotator().Quaternion()*SwiveledRot.Quaternion()).Rotator(), EBoneSpaces::ComponentSpace);
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
if (BoneAdjustments[i].bDebugDraw)
{
DrawDebugCoordinateSystem(SkelComp->GetWorld(), SkelComp->GetBoneLocation(BonesToRetarget[i].BoneName), SwiveledRot, 100.f, false, 0.1f);
}
#endif
FCompactPoseBoneIndex CompactPoseBoneToModify = BonesToRetarget[i].GetCompactPoseIndex(BoneContainer);
FTransform NewBoneTM = MeshBases.GetComponentSpaceTransform(CompactPoseBoneToModify);
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, BCS_ComponentSpace);
const FQuat BoneQuat(SwiveledRot);
NewBoneTM.SetRotation(BoneQuat);
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, BCS_ComponentSpace);
FAnimationRuntime::SetSpaceTransform(TempPose, BonesToRetarget[i].BoneIndex, NewBoneTM);
OutBoneTransforms.Add(FBoneTransform(BonesToRetarget[i].GetCompactPoseIndex(BoneContainer), NewBoneTM));
}
++i;
}
}
void FAnimNode_MMDIK::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
FVector EffectorLocation(FVector::ZeroVector);
FVector JointTargetLocation(FVector::ZeroVector);
TEnumAsByte<enum EBoneControlSpace> EffectorLocationSpace(BCS_BoneSpace);
TEnumAsByte<enum EBoneControlSpace> JointTargetLocationSpace(BCS_ParentBoneSpace);
FTransform UpperLimbCSTransform;
FTransform LowerLimbCSTransform;
FTransform EndBoneCSTransform;
FTransform JointTargetTransform;
const float BlendWeight = FMath::Clamp<float>(1.0f, 0.f, 1.f);
check(OutBoneTransforms.Num() == 0);
const FStringAssetReference& AssetRef = MMDExtendAssetRef.ToStringReference();
UMMDExtendAsset* MMDExtendAssetPtr = MMDExtendAssetRef.Get();
if (MMDExtendAssetPtr == nullptr)
{
UE_LOG(LogAnimation, Warning, TEXT("FAnimNode_MMDIK::EvaluateBoneTransforms: MMExtendPtr is nullptr!"));
return;
}
for (int32 indexIK = 0; indexIK < MMDExtendAssetPtr->IkInfoList.Num(); indexIK++)
{
JointTargetLocationSpace = BCS_ParentBoneSpace;
// Get indices of the lower and upper limb bones and check validity.
bool bInvalidLimb = false;
// IKBoneIndex
const FName EffectorSpaceBoneName = MMDExtendAssetPtr->IkInfoList[indexIK].IKBoneName;
const int32 EffectorSpaceBoneIndex = MMDExtendAssetPtr->IkInfoList[indexIK].IKBoneIndex;
const FName EndBoneName = MMDExtendAssetPtr->IkInfoList[indexIK].TargetBoneName;
const int32 EndBoneIndex = MMDExtendAssetPtr->IkInfoList[indexIK].TargetBoneIndex;
if (EffectorSpaceBoneName.IsEqual(TEXT("左つま先ＩＫ")) || EffectorSpaceBoneName.IsEqual(TEXT("右つま先ＩＫ")))
{
JointTargetLocationSpace = BCS_BoneSpace;
}
const int32 LowerLimbIndex = RequiredBones.GetParentBoneIndex(EndBoneIndex);
if (LowerLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
int32 UpperLimbIndex = INDEX_NONE;
if (!bInvalidLimb)
{
UpperLimbIndex = RequiredBones.GetParentBoneIndex(LowerLimbIndex);
if (UpperLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
}
if (!bInvalidLimb)
{
int32 JointTargetSpaceBoneIndex = INDEX_NONE;
if (MMDExtendAssetPtr->IkInfoList[indexIK].ikLinkList.Num() > 0)
{
JointTargetSpaceBoneIndex = MMDExtendAssetPtr->IkInfoList[indexIK].ikLinkList[0].BoneIndex;
}
UpperLimbCSTransform = MeshBases.GetComponentSpaceTransform(UpperLimbIndex);
LowerLimbCSTransform = MeshBases.GetComponentSpaceTransform(LowerLimbIndex);
EndBoneCSTransform = MeshBases.GetComponentSpaceTransform(EndBoneIndex);
FTransform JointTargetTransform(JointTargetLocation);
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, JointTargetTransform, JointTargetSpaceBoneIndex, JointTargetLocationSpace);
const FVector RootPos = UpperLimbCSTransform.GetTranslation();
const FVector InitialJointPos = LowerLimbCSTransform.GetTranslation();
const FVector InitialEndPos = EndBoneCSTransform.GetTranslation();
FTransform EffectorTransform(EffectorLocation);
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, EffectorTransform, EffectorSpaceBoneIndex, EffectorLocationSpace);
FVector DesiredPos = EffectorTransform.GetTranslation();
FVector DesiredDelta = DesiredPos - RootPos;
float DesiredLength = DesiredDelta.Size();
// Check to handle case where DesiredPos is the same as RootPos.
FVector DesiredDir;
if (DesiredLength < (float)KINDA_SMALL_NUMBER)
{
DesiredLength = (float)KINDA_SMALL_NUMBER;
DesiredDir = FVector(1, 0, 0);
}
else
{
DesiredDir = DesiredDelta / DesiredLength;
}
FVector JointTargetPos = JointTargetTransform.GetTranslation();
FVector JointTargetDelta = JointTargetPos - RootPos;
float JointTargetLength = JointTargetDelta.Size();
// Same check as above, to cover case when JointTarget position is the same as RootPos.
FVector JointPlaneNormal, JointBendDir;
if (JointTargetLength < (float)KINDA_SMALL_NUMBER)
{
JointBendDir = FVector(0, 1, 0);
JointPlaneNormal = FVector(0, 0, 1);
}
else
{
JointPlaneNormal = DesiredDir ^ JointTargetDelta;
// If we are trying to point the limb in the same direction that we are supposed to displace the joint in,
// we have to just pick 2 random vector perp to DesiredDir and each other.
if (JointPlaneNormal.Size() < (float)KINDA_SMALL_NUMBER)
{
DesiredDir.FindBestAxisVectors(JointPlaneNormal, JointBendDir);
}
else
{
JointPlaneNormal.Normalize();
// Find the final member of the reference frame by removing any component of JointTargetDelta along DesiredDir.
// This should never leave a zero vector, because we've checked DesiredDir and JointTargetDelta are not parallel.
JointBendDir = JointTargetDelta - ((JointTargetDelta | DesiredDir) * DesiredDir);
JointBendDir.Normalize();
}
}
// Find lengths of upper and lower limb in the ref skeleton.
// Use actual sizes instead of ref skeleton, so we take into account translation and scaling from other bone controllers.
float LowerLimbLength = (InitialEndPos - InitialJointPos).Size();
float UpperLimbLength = (InitialJointPos - RootPos).Size();
float MaxLimbLength = LowerLimbLength + UpperLimbLength;
FVector OutEndPos = DesiredPos;
FVector OutJointPos = InitialJointPos;
// If we are trying to reach a goal beyond the length of the limb, clamp it to something solvable and extend limb fully.
if (DesiredLength > MaxLimbLength)
{
OutEndPos = RootPos + (MaxLimbLength * DesiredDir);
OutJointPos = RootPos + (UpperLimbLength * DesiredDir);
}
else
{
// So we have a triangle we know the side lengths of. We can work out the angle between DesiredDir and the direction of the upper limb
// using the sin rule:
const float TwoAB = 2.f * UpperLimbLength * DesiredLength;
const float CosAngle = (TwoAB != 0.f) ? ((UpperLimbLength*UpperLimbLength) + (DesiredLength*DesiredLength) - (LowerLimbLength*LowerLimbLength)) / TwoAB : 0.f;
// If CosAngle is less than 0, the upper arm actually points the opposite way to DesiredDir, so we handle that.
const bool bReverseUpperBone = (CosAngle < 0.f);
// If CosAngle is greater than 1.f, the triangle could not be made - we cannot reach the target.
// We just have the two limbs double back on themselves, and EndPos will not equal the desired EffectorLocation.
if ((CosAngle > 1.f) || (CosAngle < -1.f))
{
// Because we want the effector to be a positive distance down DesiredDir, we go back by the smaller section.
if (UpperLimbLength > LowerLimbLength)
{
OutJointPos = RootPos + (UpperLimbLength * DesiredDir);
OutEndPos = OutJointPos - (LowerLimbLength * DesiredDir);
}
else
{
OutJointPos = RootPos - (UpperLimbLength * DesiredDir);
OutEndPos = OutJointPos + (LowerLimbLength * DesiredDir);
}
}
else
{
// Angle between upper limb and DesiredDir
const float Angle = FMath::Acos(CosAngle);
// Now we calculate the distance of the joint from the root -> effector line.
// This forms a right-angle triangle, with the upper limb as the hypotenuse.
const float JointLineDist = UpperLimbLength * FMath::Sin(Angle);
// And the final side of that triangle - distance along DesiredDir of perpendicular.
// ProjJointDistSqr can't be neg, because JointLineDist must be <= UpperLimbLength because appSin(Angle) is <= 1.
const float ProjJointDistSqr = (UpperLimbLength*UpperLimbLength) - (JointLineDist*JointLineDist);
// although this shouldn't be ever negative, sometimes Xbox release produces -0.f, causing ProjJointDist to be NaN
// so now I branch it.
float ProjJointDist = (ProjJointDistSqr>0.f) ? FMath::Sqrt(ProjJointDistSqr) : 0.f;
if (bReverseUpperBone)
{
ProjJointDist *= -1.f;
}
// So now we can work out where to put the joint!
OutJointPos = RootPos + (ProjJointDist * DesiredDir) + (JointLineDist * JointBendDir);
}
}
// Update transform for upper bone.
{
// Get difference in direction for old and new joint orientations
FVector const OldDir = (InitialJointPos - RootPos).GetSafeNormal();
FVector const NewDir = (OutJointPos - RootPos).GetSafeNormal();
// Find Delta Rotation take takes us from Old to New dir
FQuat const DeltaRotation = FQuat::FindBetween(OldDir, NewDir);
// Rotate our Joint quaternion by this delta rotation
UpperLimbCSTransform.SetRotation(DeltaRotation * UpperLimbCSTransform.GetRotation());
// And put joint where it should be.
UpperLimbCSTransform.SetTranslation(RootPos);
// Order important. First bone is upper limb.
OutBoneTransforms.Add(FBoneTransform(UpperLimbIndex, UpperLimbCSTransform));
}
// Update transform for lower bone.
{
// Get difference in direction for old and new joint orientations
FVector const OldDir = (InitialEndPos - InitialJointPos).GetSafeNormal();
FVector const NewDir = (OutEndPos - OutJointPos).GetSafeNormal();
// Find Delta Rotation take takes us from Old to New dir
FQuat const DeltaRotation = FQuat::FindBetween(OldDir, NewDir);
// Rotate our Joint quaternion by this delta rotation
LowerLimbCSTransform.SetRotation(DeltaRotation * LowerLimbCSTransform.GetRotation());
// And put joint where it should be.
LowerLimbCSTransform.SetTranslation(OutJointPos);
// Order important. Second bone is lower limb.
OutBoneTransforms.Add(FBoneTransform(LowerLimbIndex, LowerLimbCSTransform));
}
// Update transform for end bone.
{
// Set correct location for end bone.
EndBoneCSTransform.SetTranslation(OutEndPos);
// Order important. Third bone is End Bone.
OutBoneTransforms.Add(FBoneTransform(EndBoneIndex, EndBoneCSTransform));
}
OutBoneTransforms.Sort([](const FBoneTransform& A, const FBoneTransform& B)
{
return A.BoneIndex < B.BoneIndex;
});
if (OutBoneTransforms.Num() > 0)
{
MeshBases.LocalBlendCSBoneTransforms(OutBoneTransforms, BlendWeight);
OutBoneTransforms.Empty();
}
}
}
}
void FAnimNode_TwoBoneIK::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer & RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
// Get indices of the lower and upper limb bones and check validity.
bool bInvalidLimb = false;
const int32 EndBoneIndex = IKBone.BoneIndex;
const int32 LowerLimbIndex = RequiredBones.GetParentBoneIndex(EndBoneIndex);
if (LowerLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
const int32 UpperLimbIndex = RequiredBones.GetParentBoneIndex(LowerLimbIndex);
if (UpperLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
const bool bInBoneSpace = (EffectorLocationSpace == BCS_ParentBoneSpace) || (EffectorLocationSpace == BCS_BoneSpace);
const int32 EffectorSpaceBoneIndex = bInBoneSpace ? RequiredBones.GetPoseBoneIndexForBoneName(EffectorSpaceBoneName) : INDEX_NONE;
if (bInBoneSpace && ((EffectorSpaceBoneIndex == INDEX_NONE) || !RequiredBones.Contains(EffectorSpaceBoneIndex)))
{
bInvalidLimb = true;
}
// If we walked past the root, this controlled is invalid, so return no affected bones.
if( bInvalidLimb )
{
return;
}
// Get Local Space transforms for our bones. We do this first in case they already are local.
// As right after we get them in component space. (And that does the auto conversion).
// We might save one transform by doing local first...
const FTransform EndBoneLocalTransform = MeshBases.GetLocalSpaceTransform(IKBone.BoneIndex);
// Now get those in component space...
FTransform UpperLimbCSTransform = MeshBases.GetComponentSpaceTransform(UpperLimbIndex);
FTransform LowerLimbCSTransform = MeshBases.GetComponentSpaceTransform(LowerLimbIndex);
FTransform EndBoneCSTransform = MeshBases.GetComponentSpaceTransform(IKBone.BoneIndex);
// Get current position of root of limb.
// All position are in Component space.
const FVector RootPos = UpperLimbCSTransform.GetTranslation();
const FVector InitialJointPos = LowerLimbCSTransform.GetTranslation();
const FVector InitialEndPos = EndBoneCSTransform.GetTranslation();
// Transform EffectorLocation from EffectorLocationSpace to ComponentSpace.
FTransform EffectorTransform(EffectorLocation);
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, EffectorTransform, EffectorSpaceBoneIndex, EffectorLocationSpace);
// This is our reach goal.
FVector DesiredPos = EffectorTransform.GetTranslation();
FVector DesiredDelta = DesiredPos - RootPos;
float DesiredLength = DesiredDelta.Size();
// Check to handle case where DesiredPos is the same as RootPos.
FVector DesiredDir;
if (DesiredLength < (float)KINDA_SMALL_NUMBER)
{
DesiredLength = (float)KINDA_SMALL_NUMBER;
DesiredDir = FVector(1,0,0);
}
else
{
DesiredDir = DesiredDelta / DesiredLength;
}
// Get joint target (used for defining plane that joint should be in).
FTransform JointTargetTransform(JointTargetLocation);
const int32 JointTargetSpaceBoneIndex = (JointTargetLocationSpace == BCS_ParentBoneSpace || JointTargetLocationSpace == BCS_BoneSpace) ? RequiredBones.GetPoseBoneIndexForBoneName(JointTargetSpaceBoneName) : INDEX_NONE;
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, JointTargetTransform, JointTargetSpaceBoneIndex, JointTargetLocationSpace);
FVector JointTargetPos = JointTargetTransform.GetTranslation();
FVector JointTargetDelta = JointTargetPos - RootPos;
float JointTargetLength = JointTargetDelta.Size();
// Same check as above, to cover case when JointTarget position is the same as RootPos.
FVector JointPlaneNormal, JointBendDir;
if (JointTargetLength < (float)KINDA_SMALL_NUMBER)
{
JointBendDir = FVector(0,1,0);
JointPlaneNormal = FVector(0,0,1);
}
else
{
JointPlaneNormal = DesiredDir ^ JointTargetDelta;
// If we are trying to point the limb in the same direction that we are supposed to displace the joint in,
// we have to just pick 2 random vector perp to DesiredDir and each other.
if (JointPlaneNormal.Size() < (float)KINDA_SMALL_NUMBER)
{
DesiredDir.FindBestAxisVectors(JointPlaneNormal, JointBendDir);
}
else
{
JointPlaneNormal.Normalize();
// Find the final member of the reference frame by removing any component of JointTargetDelta along DesiredDir.
// This should never leave a zero vector, because we've checked DesiredDir and JointTargetDelta are not parallel.
JointBendDir = JointTargetDelta - ((JointTargetDelta | DesiredDir) * DesiredDir);
JointBendDir.Normalize();
}
}
// Find lengths of upper and lower limb in the ref skeleton.
// Use actual sizes instead of ref skeleton, so we take into account translation and scaling from other bone controllers.
float LowerLimbLength = (InitialEndPos - InitialJointPos).Size();
float UpperLimbLength = (InitialJointPos - RootPos).Size();
float MaxLimbLength = LowerLimbLength + UpperLimbLength;
if (bAllowStretching)
{
const float ScaleRange = StretchLimits.Y - StretchLimits.X;
if( ScaleRange > KINDA_SMALL_NUMBER && MaxLimbLength > KINDA_SMALL_NUMBER )
{
const float ReachRatio = DesiredLength / MaxLimbLength;
const float ScalingFactor = (StretchLimits.Y - 1.f) * FMath::Clamp<float>((ReachRatio - StretchLimits.X) / ScaleRange, 0.f, 1.f);
if (ScalingFactor > KINDA_SMALL_NUMBER)
{
LowerLimbLength *= (1.f + ScalingFactor);
UpperLimbLength *= (1.f + ScalingFactor);
MaxLimbLength *= (1.f + ScalingFactor);
}
}
}
FVector OutEndPos = DesiredPos;
FVector OutJointPos = InitialJointPos;
// If we are trying to reach a goal beyond the length of the limb, clamp it to something solvable and extend limb fully.
if (DesiredLength > MaxLimbLength)
{
OutEndPos = RootPos + (MaxLimbLength * DesiredDir);
OutJointPos = RootPos + (UpperLimbLength * DesiredDir);
}
else
{
// So we have a triangle we know the side lengths of. We can work out the angle between DesiredDir and the direction of the upper limb
// using the sin rule:
const float TwoAB = 2.f * UpperLimbLength * DesiredLength;
const float CosAngle = (TwoAB != 0.f) ? ((UpperLimbLength*UpperLimbLength) + (DesiredLength*DesiredLength) - (LowerLimbLength*LowerLimbLength)) / TwoAB : 0.f;
// If CosAngle is less than 0, the upper arm actually points the opposite way to DesiredDir, so we handle that.
const bool bReverseUpperBone = (CosAngle < 0.f);
// If CosAngle is greater than 1.f, the triangle could not be made - we cannot reach the target.
// We just have the two limbs double back on themselves, and EndPos will not equal the desired EffectorLocation.
if ((CosAngle > 1.f) || (CosAngle < -1.f))
{
// Because we want the effector to be a positive distance down DesiredDir, we go back by the smaller section.
if (UpperLimbLength > LowerLimbLength)
{
OutJointPos = RootPos + (UpperLimbLength * DesiredDir);
OutEndPos = OutJointPos - (LowerLimbLength * DesiredDir);
}
else
{
OutJointPos = RootPos - (UpperLimbLength * DesiredDir);
OutEndPos = OutJointPos + (LowerLimbLength * DesiredDir);
}
}
else
{
// Angle between upper limb and DesiredDir
const float Angle = FMath::Acos(CosAngle);
// Now we calculate the distance of the joint from the root -> effector line.
// This forms a right-angle triangle, with the upper limb as the hypotenuse.
const float JointLineDist = UpperLimbLength * FMath::Sin(Angle);
// And the final side of that triangle - distance along DesiredDir of perpendicular.
// ProjJointDistSqr can't be neg, because JointLineDist must be <= UpperLimbLength because appSin(Angle) is <= 1.
const float ProjJointDistSqr = (UpperLimbLength*UpperLimbLength) - (JointLineDist*JointLineDist);
// although this shouldn't be ever negative, sometimes Xbox release produces -0.f, causing ProjJointDist to be NaN
// so now I branch it.
float ProjJointDist = (ProjJointDistSqr>0.f)? FMath::Sqrt(ProjJointDistSqr) : 0.f;
if( bReverseUpperBone )
{
ProjJointDist *= -1.f;
}
// So now we can work out where to put the joint!
OutJointPos = RootPos + (ProjJointDist * DesiredDir) + (JointLineDist * JointBendDir);
}
}
// Update transform for upper bone.
{
// Get difference in direction for old and new joint orientations
FVector const OldDir = (InitialJointPos - RootPos).SafeNormal();
FVector const NewDir = (OutJointPos - RootPos).SafeNormal();
// Find Delta Rotation take takes us from Old to New dir
FQuat const DeltaRotation = FQuat::FindBetween(OldDir, NewDir);
// Rotate our Joint quaternion by this delta rotation
UpperLimbCSTransform.SetRotation( DeltaRotation * UpperLimbCSTransform.GetRotation() );
// And put joint where it should be.
UpperLimbCSTransform.SetTranslation( RootPos );
// Order important. First bone is upper limb.
OutBoneTransforms.Add( FBoneTransform(UpperLimbIndex, UpperLimbCSTransform) );
}
// Update transform for lower bone.
{
// Get difference in direction for old and new joint orientations
FVector const OldDir = (InitialEndPos - InitialJointPos).SafeNormal();
FVector const NewDir = (OutEndPos - OutJointPos).SafeNormal();
// Find Delta Rotation take takes us from Old to New dir
FQuat const DeltaRotation = FQuat::FindBetween(OldDir, NewDir);
// Rotate our Joint quaternion by this delta rotation
LowerLimbCSTransform.SetRotation( DeltaRotation * LowerLimbCSTransform.GetRotation() );
// And put joint where it should be.
LowerLimbCSTransform.SetTranslation( OutJointPos );
// Order important. Second bone is lower limb.
OutBoneTransforms.Add( FBoneTransform(LowerLimbIndex, LowerLimbCSTransform) );
}
// Update transform for end bone.
{
if( bTakeRotationFromEffectorSpace )
{
EndBoneCSTransform.SetRotation( EffectorTransform.GetRotation() );
}
else if( bMaintainEffectorRelRot )
{
EndBoneCSTransform = EndBoneLocalTransform * LowerLimbCSTransform;
}
// Set correct location for end bone.
EndBoneCSTransform.SetTranslation(OutEndPos);
// Order important. Third bone is End Bone.
OutBoneTransforms.Add( FBoneTransform(IKBone.BoneIndex, EndBoneCSTransform) );
}
// Make sure we have correct number of bones
check(OutBoneTransforms.Num() == 3);
}
void FAnimNode_AnimDynamics::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
SCOPE_CYCLE_COUNTER(STAT_AnimDynamicsOverall);
int32 RestrictToLOD = CVarRestrictLod.GetValueOnAnyThread();
bool bEnabledForLod = RestrictToLOD >= 0 ? SkelComp->PredictedLODLevel == RestrictToLOD : true;
if (CVarEnableDynamics.GetValueOnAnyThread() == 1 && bEnabledForLod)
{
// Pretty nasty - but there isn't really a good way to get clean bone transforms (without the modification from
// previous runs) so we have to initialize here, checking often so we can restart a simulation in the editor.
if (bRequiresInit)
{
InitPhysics(SkelComp, MeshBases);
bRequiresInit = false;
}
if (bDoUpdate && NextTimeStep > 0.0f)
{
// Wind / Force update
if(CVarEnableWind.GetValueOnAnyThread() == 1 && bEnableWind)
{
SCOPE_CYCLE_COUNTER(STAT_AnimDynamicsWindData);
for(FAnimPhysRigidBody* Body : BaseBodyPtrs)
{
if(SkelComp && SkelComp->GetWorld())
{
Body->bWindEnabled = bEnableWind;
if(Body->bWindEnabled)
{
UWorld* World = SkelComp->GetWorld();
FSceneInterface* Scene = World->Scene;
// Unused by our simulation but needed for the call to GetWindParameters below
float WindMinGust;
float WindMaxGust;
// Setup wind data
Body->bWindEnabled = true;
Scene->GetWindParameters(SkelComp->ComponentToWorld.TransformPosition(Body->Pose.Position), Body->WindData.WindDirection, Body->WindData.WindSpeed, WindMinGust, WindMaxGust);
Body->WindData.WindDirection = SkelComp->ComponentToWorld.Inverse().TransformVector(Body->WindData.WindDirection);
Body->WindData.WindAdaption = FMath::FRandRange(0.0f, 2.0f);
Body->WindData.BodyWindScale = WindScale;
}
}
}
}
else
{
SCOPE_CYCLE_COUNTER(STAT_AnimDynamicsWindData);
// Disable wind.
for(FAnimPhysRigidBody* Body : BaseBodyPtrs)
{
Body->bWindEnabled = false;
}
}
if (CVarEnableAdaptiveSubstep.GetValueOnAnyThread() == 1)
{
float FixedTimeStep = MaxSubstepDeltaTime * CurrentTimeDilation;
// Clamp the fixed timestep down to max physics tick time.
// at high speeds the simulation will not converge as the delta time is too high, this will
// help to keep constraints together at a cost of physical accuracy
FixedTimeStep = FMath::Clamp(FixedTimeStep, 0.0f, MaxPhysicsDeltaTime);
// Calculate number of substeps we should do.
int32 NumIters = FMath::TruncToInt((NextTimeStep + (TimeDebt * CurrentTimeDilation)) / FixedTimeStep);
NumIters = FMath::Clamp(NumIters, 0, MaxSubsteps);
SET_DWORD_STAT(STAT_AnimDynamicsSubSteps, NumIters);
// Store the remaining time as debt for later frames
TimeDebt = (NextTimeStep + TimeDebt) - (NumIters * FixedTimeStep);
TimeDebt = FMath::Clamp(TimeDebt, 0.0f, MaxTimeDebt);
NextTimeStep = FixedTimeStep;
for (int32 Iter = 0; Iter < NumIters; ++Iter)
{
UpdateLimits(SkelComp, MeshBases);
FAnimPhys::PhysicsUpdate(FixedTimeStep, BaseBodyPtrs, LinearLimits, AngularLimits, Springs, NumSolverIterationsPreUpdate, NumSolverIterationsPostUpdate);
}
}
else
{
// Do variable frame-time update
const float MaxDeltaTime = MaxPhysicsDeltaTime;
NextTimeStep = FMath::Min(NextTimeStep, MaxDeltaTime);
UpdateLimits(SkelComp, MeshBases);
FAnimPhys::PhysicsUpdate(NextTimeStep, BaseBodyPtrs, LinearLimits, AngularLimits, Springs, NumSolverIterationsPreUpdate, NumSolverIterationsPostUpdate);
}
}
if (bDoEval)
{
SCOPE_CYCLE_COUNTER(STAT_AnimDynamicsBoneEval);
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
for (int32 Idx = 0; Idx < BoundBoneReferences.Num(); ++Idx)
{
FBoneReference& CurrentChainBone = BoundBoneReferences[Idx];
FAnimPhysRigidBody& CurrentBody = Bodies[Idx].RigidBody.PhysBody;
// Skip invalid bones
if(!CurrentChainBone.IsValid(BoneContainer))
{
continue;
}
FCompactPoseBoneIndex BoneIndex = CurrentChainBone.GetCompactPoseIndex(BoneContainer);
FTransform NewBoneTransform(CurrentBody.Pose.Orientation, CurrentBody.Pose.Position + CurrentBody.Pose.Orientation.RotateVector(JointOffsets[Idx]));
OutBoneTransforms.Add(FBoneTransform(BoneIndex, NewBoneTransform));
}
}
}
}
void FAnimNode_SpringBone::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
// Location of our bone in world space
FTransform SpaceBase = MeshBases.GetComponentSpaceTransform(SpringBone.BoneIndex);
FTransform BoneTransformInWorldSpace = (SkelComp != NULL) ? SpaceBase * SkelComp->GetComponentToWorld() : SpaceBase;
FVector const TargetPos = BoneTransformInWorldSpace.GetLocation();
AActor* SkelOwner = (SkelComp != NULL) ? SkelComp->GetOwner() : NULL;
if ((SkelComp != NULL) && (SkelComp->AttachParent != NULL) && (SkelOwner == NULL))
{
SkelOwner = SkelComp->AttachParent->GetOwner();
}
// Init values first time
if (RemainingTime == 0.0f)
{
BoneLocation = TargetPos;
BoneVelocity = FVector::ZeroVector;
}
while (RemainingTime > FixedTimeStep)
{
// Update location of our base by how much our base moved this frame.
FVector const BaseTranslation = SkelOwner ? (SkelOwner->GetVelocity() * FixedTimeStep) : FVector::ZeroVector;
BoneLocation += BaseTranslation;
// Reinit values if outside reset threshold
if (((TargetPos - BoneLocation).SizeSquared() > (ErrorResetThresh*ErrorResetThresh)))
{
BoneLocation = TargetPos;
BoneVelocity = FVector::ZeroVector;
}
// Calculate error vector.
FVector const Error = (TargetPos - BoneLocation);
FVector const DampingForce = SpringDamping * BoneVelocity;
FVector const SpringForce = SpringStiffness * Error;
// Calculate force based on error and vel
FVector const Acceleration = SpringForce - DampingForce;
// Integrate velocity
// Make sure damping with variable frame rate actually dampens velocity. Otherwise Spring will go nuts.
float const CutOffDampingValue = 1.f/FixedTimeStep;
if (SpringDamping > CutOffDampingValue)
{
float const SafetyScale = CutOffDampingValue / SpringDamping;
BoneVelocity += SafetyScale * (Acceleration * FixedTimeStep);
}
else
{
BoneVelocity += (Acceleration * FixedTimeStep);
}
// Clamp velocity to something sane (|dX/dt| <= ErrorResetThresh)
float const BoneVelocityMagnitude = BoneVelocity.Size();
if (BoneVelocityMagnitude * FixedTimeStep > ErrorResetThresh)
{
BoneVelocity *= (ErrorResetThresh / (BoneVelocityMagnitude * FixedTimeStep));
}
// Integrate position
FVector const OldBoneLocation = BoneLocation;
FVector const DeltaMove = (BoneVelocity * FixedTimeStep);
BoneLocation += DeltaMove;
// Force z to be correct if desired
if (bNoZSpring)
{
BoneLocation.Z = TargetPos.Z;
}
// If desired, limit error
if (bLimitDisplacement)
{
FVector CurrentDisp = BoneLocation - TargetPos;
// Too far away - project back onto sphere around target.
if (CurrentDisp.Size() > MaxDisplacement)
{
FVector DispDir = CurrentDisp.GetSafeNormal();
BoneLocation = TargetPos + (MaxDisplacement * DispDir);
}
}
// Update velocity to reflect post processing done to bone location.
BoneVelocity = (BoneLocation - OldBoneLocation) / FixedTimeStep;
check( !BoneLocation.ContainsNaN() );
check( !BoneVelocity.ContainsNaN() );
RemainingTime -= FixedTimeStep;
}
// Now convert back into component space and output - rotation is unchanged.
FTransform OutBoneTM = SpaceBase;
OutBoneTM.SetLocation( SkelComp->GetComponentToWorld().InverseTransformPosition(BoneLocation) );
// Output new transform for current bone.
OutBoneTransforms.Add( FBoneTransform(SpringBone.BoneIndex, OutBoneTM) );
}
void FAnimNode_Fabrik::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
// IsValidToEvaluate validated our inputs, we don't need to check pre-requisites again.
int32 const RootIndex = RootBone.BoneIndex;
// Update EffectorLocation if it is based off a bone position
FTransform CSEffectorTransform = FTransform(EffectorTransform);
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, CSEffectorTransform, EffectorTransformBone.BoneIndex, EffectorTransformSpace);
FVector const CSEffectorLocation = CSEffectorTransform.GetLocation();
// @fixme - we need better to draw widgets and debug information in editor.
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
if (bEnableDebugDraw)
{
// Show end effector position.
DrawDebugBox(SkelComp->GetWorld(), CSEffectorLocation, FVector(Precision), FColor::Green, true, 0.1f);
DrawDebugCoordinateSystem(SkelComp->GetWorld(), CSEffectorLocation, CSEffectorTransform.GetRotation().Rotator(), 5.f, true, 0.1f);
}
#endif
// Gather all bone indices between root and tip.
TArray<int32> BoneIndices;
{
int32 BoneIndex = TipBone.BoneIndex;
do
{
BoneIndices.Insert(BoneIndex, 0);
BoneIndex = RequiredBones.GetParentBoneIndex(BoneIndex);
} while (BoneIndex != RootIndex);
BoneIndices.Insert(BoneIndex, 0);
}
// Maximum length of skeleton segment at full extension
float MaximumReach = 0;
// Gather transforms
int32 const NumTransforms = BoneIndices.Num();
OutBoneTransforms.AddUninitialized(NumTransforms);
// Gather chain links. These are non zero length bones.
TArray<FABRIKChainLink> Chain;
Chain.Reserve(NumTransforms);
// Start with Root Bone
{
int32 const & RootBoneIndex = BoneIndices[0];
FTransform const BoneCSTransform = MeshBases.GetComponentSpaceTransform(RootBoneIndex);
OutBoneTransforms[0] = FBoneTransform(RootBoneIndex, BoneCSTransform);
Chain.Add(FABRIKChainLink(BoneCSTransform.GetLocation(), 0.f, RootBoneIndex, 0));
}
// Go through remaining transforms
for (int32 TransformIndex = 1; TransformIndex < NumTransforms; TransformIndex++)
{
int32 const & BoneIndex = BoneIndices[TransformIndex];
FTransform const BoneCSTransform = MeshBases.GetComponentSpaceTransform(BoneIndex);
FVector const BoneCSPosition = BoneCSTransform.GetLocation();
OutBoneTransforms[TransformIndex] = FBoneTransform(BoneIndex, BoneCSTransform);
// Calculate the combined length of this segment of skeleton
float const BoneLength = FVector::Dist(BoneCSPosition, OutBoneTransforms[TransformIndex-1].Transform.GetLocation());
if (!FMath::IsNearlyZero(BoneLength))
{
Chain.Add(FABRIKChainLink(BoneCSPosition, BoneLength, BoneIndex, TransformIndex));
MaximumReach += BoneLength;
}
else
{
// Mark this transform as a zero length child of the last link.
// It will inherit position and delta rotation from parent link.
FABRIKChainLink & ParentLink = Chain[Chain.Num()-1];
ParentLink.ChildZeroLengthTransformIndices.Add(TransformIndex);
}
}
bool bBoneLocationUpdated = false;
float const RootToTargetDist = FVector::Dist(Chain[0].Position, CSEffectorLocation);
int32 const NumChainLinks = Chain.Num();
// FABRIK algorithm - bone translation calculation
// If the effector is further away than the distance from root to tip, simply move all bones in a line from root to effector location
if (RootToTargetDist > MaximumReach)
{
for (int32 LinkIndex = 1; LinkIndex < NumChainLinks; LinkIndex++)
{
FABRIKChainLink const & ParentLink = Chain[LinkIndex - 1];
FABRIKChainLink & CurrentLink = Chain[LinkIndex];
CurrentLink.Position = ParentLink.Position + (CSEffectorLocation - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length;
}
bBoneLocationUpdated = true;
}
else // Effector is within reach, calculate bone translations to position tip at effector location
{
int32 const TipBoneLinkIndex = NumChainLinks - 1;
// Check distance between tip location and effector location
float Slop = FVector::Dist(Chain[TipBoneLinkIndex].Position, CSEffectorLocation);
if (Slop > Precision)
{
// Set tip bone at end effector location.
Chain[TipBoneLinkIndex].Position = CSEffectorLocation;
int32 IterationCount = 0;
while ((Slop > Precision) && (IterationCount++ < MaxIterations))
{
// "Forward Reaching" stage - adjust bones from end effector.
for (int32 LinkIndex = TipBoneLinkIndex - 1; LinkIndex > 0; LinkIndex--)
{
FABRIKChainLink & CurrentLink = Chain[LinkIndex];
FABRIKChainLink const & ChildLink = Chain[LinkIndex + 1];
CurrentLink.Position = ChildLink.Position + (CurrentLink.Position - ChildLink.Position).GetUnsafeNormal() * ChildLink.Length;
}
// "Backward Reaching" stage - adjust bones from root.
for (int32 LinkIndex = 1; LinkIndex < TipBoneLinkIndex; LinkIndex++)
{
FABRIKChainLink const & ParentLink = Chain[LinkIndex - 1];
FABRIKChainLink & CurrentLink = Chain[LinkIndex];
CurrentLink.Position = ParentLink.Position + (CurrentLink.Position - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length;
}
// Re-check distance between tip location and effector location
// Since we're keeping tip on top of effector location, check with its parent bone.
Slop = FMath::Abs(Chain[TipBoneLinkIndex].Length - FVector::Dist(Chain[TipBoneLinkIndex - 1].Position, CSEffectorLocation));
}
// Place tip bone based on how close we got to target.
{
FABRIKChainLink const & ParentLink = Chain[TipBoneLinkIndex - 1];
FABRIKChainLink & CurrentLink = Chain[TipBoneLinkIndex];
CurrentLink.Position = ParentLink.Position + (CurrentLink.Position - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length;
}
bBoneLocationUpdated = true;
}
}
// If we moved some bones, update bone transforms.
if (bBoneLocationUpdated)
{
// First step: update bone transform positions from chain links.
for (int32 LinkIndex = 0; LinkIndex < NumChainLinks; LinkIndex++)
{
FABRIKChainLink const & ChainLink = Chain[LinkIndex];
OutBoneTransforms[ChainLink.TransformIndex].Transform.SetTranslation(ChainLink.Position);
// If there are any zero length children, update position of those
int32 const NumChildren = ChainLink.ChildZeroLengthTransformIndices.Num();
for (int32 ChildIndex = 0; ChildIndex < NumChildren; ChildIndex++)
{
OutBoneTransforms[ChainLink.ChildZeroLengthTransformIndices[ChildIndex]].Transform.SetTranslation(ChainLink.Position);
}
}
// FABRIK algorithm - re-orientation of bone local axes after translation calculation
for (int32 LinkIndex = 0; LinkIndex < NumChainLinks - 1; LinkIndex++)
{
FABRIKChainLink const & CurrentLink = Chain[LinkIndex];
FABRIKChainLink const & ChildLink = Chain[LinkIndex + 1];
// Calculate pre-translation vector between this bone and child
FVector const OldDir = (GetCurrentLocation(MeshBases, ChildLink.BoneIndex) - GetCurrentLocation(MeshBases, CurrentLink.BoneIndex)).GetUnsafeNormal();
// Get vector from the post-translation bone to it's child
FVector const NewDir = (ChildLink.Position - CurrentLink.Position).GetUnsafeNormal();
// Calculate axis of rotation from pre-translation vector to post-translation vector
FVector const RotationAxis = FVector::CrossProduct(OldDir, NewDir).GetSafeNormal();
float const RotationAngle = FMath::Acos(FVector::DotProduct(OldDir, NewDir));
FQuat const DeltaRotation = FQuat(RotationAxis, RotationAngle);
// We're going to multiply it, in order to not have to re-normalize the final quaternion, it has to be a unit quaternion.
checkSlow(DeltaRotation.IsNormalized());
// Calculate absolute rotation and set it
FTransform& CurrentBoneTransform = OutBoneTransforms[CurrentLink.TransformIndex].Transform;
CurrentBoneTransform.SetRotation(DeltaRotation * CurrentBoneTransform.GetRotation());
// Update zero length children if any
int32 const NumChildren = CurrentLink.ChildZeroLengthTransformIndices.Num();
for (int32 ChildIndex = 0; ChildIndex < NumChildren; ChildIndex++)
{
FTransform& ChildBoneTransform = OutBoneTransforms[CurrentLink.ChildZeroLengthTransformIndices[ChildIndex]].Transform;
ChildBoneTransform.SetRotation(DeltaRotation * ChildBoneTransform.GetRotation());
}
}
}
// Special handling for tip bone's rotation.
int32 const TipBoneTransformIndex = OutBoneTransforms.Num() - 1;
switch (EffectorRotationSource)
{
case BRS_KeepLocalSpaceRotation:
OutBoneTransforms[TipBoneTransformIndex].Transform = MeshBases.GetLocalSpaceTransform(BoneIndices[TipBoneTransformIndex]) * OutBoneTransforms[TipBoneTransformIndex - 1].Transform;
break;
case BRS_CopyFromTarget:
OutBoneTransforms[TipBoneTransformIndex].Transform.SetRotation(CSEffectorTransform.GetRotation());
break;
case BRS_KeepComponentSpaceRotation:
// Don't change the orientation at all
break;
default:
break;
}
}
void FAnimNode_ModifyFinger::EvaluateOneBoneTransforms(FBoneReference* BoneToModify, FRotator* Rotation, const FBoneContainer BoneContainer, FCSPose<FCompactPose> &MeshBases, USkeletalMeshComponent* SkelComp, TArray<FBoneTransform> &OutBoneTransforms)
{
FCompactPoseBoneIndex CompactPoseBoneToModify = BoneToModify->GetCompactPoseIndex(BoneContainer);
FTransform NewBoneTM = MeshBases.GetComponentSpaceTransform(CompactPoseBoneToModify);
if (RotationMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, RotationSpace);
const FQuat BoneQuat(*Rotation);
if (RotationMode == BMM_Additive)
{
NewBoneTM.SetRotation(BoneQuat * NewBoneTM.GetRotation());
}
else
{
NewBoneTM.SetRotation(BoneQuat);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, RotationSpace);
}
if (ScaleMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, ScaleSpace);
if (ScaleMode == BMM_Additive)
{
NewBoneTM.SetScale3D(NewBoneTM.GetScale3D() * Scale);
}
else
{
NewBoneTM.SetScale3D(Scale);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, ScaleSpace);
}
if (TranslationMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, TranslationSpace);
if (TranslationMode == BMM_Additive)
{
NewBoneTM.AddToTranslation(Translation1);
}
else
{
NewBoneTM.SetTranslation(Translation1);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, TranslationSpace);
}
OutBoneTransforms.Add(FBoneTransform(BoneToModify->GetCompactPoseIndex(BoneContainer), NewBoneTM));
const float BlendWeight = FMath::Clamp<float>(Alpha, 0.f, 1.f);
MeshBases.LocalBlendCSBoneTransforms(OutBoneTransforms, BlendWeight);
OutBoneTransforms.Empty();
}
void FAnimNode_ModifyBone::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
// the way we apply transform is same as FMatrix or FTransform
// we apply scale first, and rotation, and translation
// if you'd like to translate first, you'll need two nodes that first node does translate and second nodes to rotate.
const FBoneContainer BoneContainer = MeshBases.GetPose().GetBoneContainer();
FCompactPoseBoneIndex CompactPoseBoneToModify = BoneToModify.GetCompactPoseIndex(BoneContainer);
FTransform NewBoneTM = MeshBases.GetComponentSpaceTransform(CompactPoseBoneToModify);
if (ScaleMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, ScaleSpace);
if (ScaleMode == BMM_Additive)
{
NewBoneTM.SetScale3D(NewBoneTM.GetScale3D() * Scale);
}
else
{
NewBoneTM.SetScale3D(Scale);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, ScaleSpace);
}
if (RotationMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, RotationSpace);
const FQuat BoneQuat(Rotation);
if (RotationMode == BMM_Additive)
{
NewBoneTM.SetRotation(BoneQuat * NewBoneTM.GetRotation());
}
else
{
NewBoneTM.SetRotation(BoneQuat);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, RotationSpace);
}
if (TranslationMode != BMM_Ignore)
{
// Convert to Bone Space.
FAnimationRuntime::ConvertCSTransformToBoneSpace(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, TranslationSpace);
if (TranslationMode == BMM_Additive)
{
NewBoneTM.AddToTranslation(Translation);
}
else
{
NewBoneTM.SetTranslation(Translation);
}
// Convert back to Component Space.
FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, NewBoneTM, CompactPoseBoneToModify, TranslationSpace);
}
OutBoneTransforms.Add( FBoneTransform(BoneToModify.GetCompactPoseIndex(BoneContainer), NewBoneTM) );
}
