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_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_ModifyFinger::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();
// For all bones
EvaluateOneBoneTransforms(&TargetFingerBone1, &Rotation1, BoneContainer, MeshBases, SkelComp, OutBoneTransforms);
EvaluateOneBoneTransforms(&TargetFingerBone2, &Rotation2, BoneContainer, MeshBases, SkelComp, OutBoneTransforms);
EvaluateOneBoneTransforms(&TargetFingerBone3, &Rotation3, BoneContainer, MeshBases, SkelComp, OutBoneTransforms);
}
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) );
}
}
}
/** Convert a BoneSpace FTransform to ComponentSpace. */
void FAnimationRuntime::ConvertBoneSpaceTransformToCS
(
USkeletalMeshComponent * SkelComp,
FCSPose<FCompactPose> & MeshBases,
/*inout*/ FTransform& BoneSpaceTM,
FCompactPoseBoneIndex BoneIndex,
uint8 Space
)
{
switch( Space )
{
case BCS_WorldSpace :
BoneSpaceTM.SetToRelativeTransform(SkelComp->ComponentToWorld);
break;
case BCS_ComponentSpace :
// Component Space, no change.
break;
case BCS_ParentBoneSpace :
if( BoneIndex != INDEX_NONE )
{
const FCompactPoseBoneIndex ParentIndex = MeshBases.GetPose().GetParentBoneIndex(BoneIndex);
if( ParentIndex != INDEX_NONE )
{
const FTransform& ParentTM = MeshBases.GetComponentSpaceTransform(ParentIndex);
BoneSpaceTM *= ParentTM;
}
}
break;
case BCS_BoneSpace :
if( BoneIndex != INDEX_NONE )
{
const FTransform& BoneTM = MeshBases.GetComponentSpaceTransform(BoneIndex);
BoneSpaceTM *= BoneTM;
}
break;
default:
UE_LOG(LogAnimation, Warning, TEXT("ConvertBoneSpaceTransformToCS: Unknown BoneSpace %d for Mesh: %s"), Space, *GetNameSafe(SkelComp->SkeletalMesh));
break;
}
}
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_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_Trail::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& 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.
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
FCompactPoseBoneIndex WalkBoneIndex = TrailBone.GetCompactPoseIndex(BoneContainer);
TArray<FCompactPoseBoneIndex> 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 = BoneContainer.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++)
{
FCompactPoseBoneIndex ChildIndex = ChainBoneIndices[i];
const 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()->GetActorQuat(), 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.
FCompactPoseBoneIndex RootIndex = ChainBoneIndices[0];
const 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.
FCompactPoseBoneIndex ParentIndex = ChainBoneIndices[i - 1];
FVector ParentPos = TrailBoneLocations[i-1];
FVector ParentAnimPos = MeshBases.GetComponentSpaceTransform(ParentIndex).GetTranslation();
// Child bone position in component space.
FCompactPoseBoneIndex 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.GetSafeNormal(SMALL_NUMBER);
// Calculate vector from parent to child.
FVector NewBoneDir = FVector(OutBoneTransforms[i].Transform.GetTranslation() - OutBoneTransforms[i - 1].Transform.GetTranslation()).GetSafeNormal(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_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_AnimDynamics::UpdateLimits(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases)
{
SCOPE_CYCLE_COUNTER(STAT_AnimDynamicsLimitUpdate);
// We're always going to use the same number so don't realloc
LinearLimits.Empty(LinearLimits.Num());
AngularLimits.Empty(AngularLimits.Num());
Springs.Empty(Springs.Num());
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
for (int32 Idx = 0; Idx < Bodies.Num(); ++Idx)
{
const FBoneReference& CurrentBoneRef = BoundBoneReferences[Idx];
// If our bone isn't valid, move on
if(!CurrentBoneRef.IsValid(BoneContainer))
{
continue;
}
FAnimPhysLinkedBody& ChainBody = Bodies[Idx];
FAnimPhysRigidBody& RigidBody = Bodies[Idx].RigidBody.PhysBody;
FAnimPhysRigidBody* PrevBody = nullptr;
if (ChainBody.ParentBody)
{
PrevBody = &ChainBody.ParentBody->PhysBody;
}
// Get joint transform
FCompactPoseBoneIndex BoneIndex = CurrentBoneRef.GetCompactPoseIndex(BoneContainer);
FTransform BoundBoneTransform = MeshBases.GetComponentSpaceTransform(BoneIndex);
FTransform ShapeTransform = BoundBoneTransform;
// Local offset to joint for Body1
FVector Body1JointOffset = LocalJointOffset;
if (PrevBody)
{
// Get the correct offset
Body1JointOffset = JointOffsets[Idx];
// Modify the shape transform to be correct in Body0 frame
ShapeTransform = FTransform(FQuat::Identity, -Body1JointOffset);
}
if (ConstraintSetup.bLinearFullyLocked)
{
// Rather than calculate prismatic limits, just lock the transform (1 limit instead of 6)
FAnimPhys::ConstrainPositionNailed(NextTimeStep, LinearLimits, PrevBody, ShapeTransform.GetTranslation(), &RigidBody, Body1JointOffset);
}
else
{
if (ConstraintSetup.LinearXLimitType != AnimPhysLinearConstraintType::Free)
{
FAnimPhys::ConstrainAlongDirection(NextTimeStep, LinearLimits, PrevBody, ShapeTransform.GetTranslation(), &RigidBody, Body1JointOffset, ShapeTransform.GetRotation().GetAxisX(), FVector2D(ConstraintSetup.LinearAxesMin.X, ConstraintSetup.LinearAxesMax.X));
}
if (ConstraintSetup.LinearYLimitType != AnimPhysLinearConstraintType::Free)
{
FAnimPhys::ConstrainAlongDirection(NextTimeStep, LinearLimits, PrevBody, ShapeTransform.GetTranslation(), &RigidBody, Body1JointOffset, ShapeTransform.GetRotation().GetAxisY(), FVector2D(ConstraintSetup.LinearAxesMin.Y, ConstraintSetup.LinearAxesMax.Y));
}
if (ConstraintSetup.LinearZLimitType != AnimPhysLinearConstraintType::Free)
{
FAnimPhys::ConstrainAlongDirection(NextTimeStep, LinearLimits, PrevBody, ShapeTransform.GetTranslation(), &RigidBody, Body1JointOffset, ShapeTransform.GetRotation().GetAxisZ(), FVector2D(ConstraintSetup.LinearAxesMin.Z, ConstraintSetup.LinearAxesMax.Z));
}
}
if (ConstraintSetup.AngularConstraintType == AnimPhysAngularConstraintType::Angular)
{
#if WITH_EDITOR
// Check the ranges are valid when running in the editor, log if something is wrong
if(ConstraintSetup.AngularLimitsMin.X > ConstraintSetup.AngularLimitsMax.X ||
ConstraintSetup.AngularLimitsMin.Y > ConstraintSetup.AngularLimitsMax.Y ||
ConstraintSetup.AngularLimitsMin.Z > ConstraintSetup.AngularLimitsMax.Z)
{
UE_LOG(LogAnimation, Warning, TEXT("AnimDynamics: Min/Max angular limits for bone %s incorrect, at least one min axis value is greater than the corresponding max."), *BoundBone.BoneName.ToString());
}
#endif
// Add angular limits. any limit with 360+ degree range is ignored and left free.
FAnimPhys::ConstrainAngularRange(NextTimeStep, AngularLimits, PrevBody, &RigidBody, ShapeTransform.GetRotation(), ConstraintSetup.TwistAxis, ConstraintSetup.AngularLimitsMin, ConstraintSetup.AngularLimitsMax);
}
else
{
FAnimPhys::ConstrainConeAngle(NextTimeStep, AngularLimits, PrevBody, BoundBoneTransform.GetRotation().GetAxisX(), &RigidBody, FVector(1.0f, 0.0f, 0.0f), ConstraintSetup.ConeAngle);
}
if(PlanarLimits.Num() > 0)
{
for(FAnimPhysPlanarLimit& PlanarLimit : PlanarLimits)
{
FTransform LimitPlaneTransform = PlanarLimit.PlaneTransform;
if(PlanarLimit.DrivingBone.IsValid(BoneContainer))
{
FCompactPoseBoneIndex DrivingBoneIndex = PlanarLimit.DrivingBone.GetCompactPoseIndex(BoneContainer);
LimitPlaneTransform *= MeshBases.GetComponentSpaceTransform(DrivingBoneIndex);// * LimitPlaneTransform;
}
FAnimPhys::ConstrainPlanar(NextTimeStep, LinearLimits, &RigidBody, LimitPlaneTransform);
}
}
// Add spring if we need spring forces
if (bAngularSpring || bLinearSpring)
{
FAnimPhys::CreateSpring(Springs, PrevBody, ShapeTransform.GetTranslation(), &RigidBody, FVector::ZeroVector);
FAnimPhysSpring& NewSpring = Springs.Last();
NewSpring.SpringConstantLinear = LinearSpringConstant;
NewSpring.SpringConstantAngular = AngularSpringConstant;
NewSpring.AngularTarget = ConstraintSetup.AngularTarget.GetSafeNormal();
NewSpring.AngularTargetAxis = ConstraintSetup.AngularTargetAxis;
NewSpring.TargetOrientationOffset = ShapeTransform.GetRotation();
NewSpring.bApplyAngular = bAngularSpring;
NewSpring.bApplyLinear = bLinearSpring;
}
}
}
void FAnimNode_AnimDynamics::InitPhysics(USkeletalMeshComponent* Component, FCSPose<FCompactPose>& MeshBases)
{
// Clear up any existing physics data
TermPhysics();
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
// List of bone indices in the chain.
TArray<int32> ChainBoneIndices;
TArray<FName> ChainBoneNames;
if(ChainEnd.IsValid(BoneContainer))
{
// Add the end of the chain. We have to walk from the bottom upwards to find a chain
// as walking downwards doesn't guarantee a single end point.
ChainBoneIndices.Add(ChainEnd.BoneIndex);
ChainBoneNames.Add(ChainEnd.BoneName);
int32 ParentBoneIndex = BoneContainer.GetParentBoneIndex(ChainEnd.BoneIndex);
// Walk up the chain until we either find the top or hit the root bone
while(ParentBoneIndex != 0)
{
ChainBoneIndices.Add(ParentBoneIndex);
ChainBoneNames.Add(Component->GetBoneName(ParentBoneIndex));
if(ParentBoneIndex == BoundBone.BoneIndex)
{
// Found the top of the chain
break;
}
ParentBoneIndex = BoneContainer.GetParentBoneIndex(ParentBoneIndex);
}
// Bail if we can't find a chain, and let the user know
if(ParentBoneIndex != BoundBone.BoneIndex)
{
UE_LOG(LogAnimation, Error, TEXT("AnimDynamics: Attempted to find bone chain starting at %s and ending at %s but failed."), *BoundBone.BoneName.ToString(), *ChainEnd.BoneName.ToString());
return;
}
}
else
{
// No chain specified, just use the bound bone
ChainBoneIndices.Add(BoundBone.BoneIndex);
ChainBoneNames.Add(BoundBone.BoneName);
}
Bodies.Reserve(ChainBoneIndices.Num());
// Walk backwards here as the chain was discovered in reverse order
for (int32 Idx = ChainBoneIndices.Num() - 1; Idx >= 0; --Idx)
{
TArray<FAnimPhysShape> BodyShapes;
BodyShapes.Add(FAnimPhysShape::MakeBox(BoxExtents));
FBoneReference LinkBoneRef;
LinkBoneRef.BoneName = ChainBoneNames[Idx];
LinkBoneRef.Initialize(BoneContainer);
// Calculate joint offsets by looking at the length of the bones and extending the provided offset
if (BoundBoneReferences.Num() > 0)
{
FTransform CurrentBoneTransform = MeshBases.GetComponentSpaceTransform(LinkBoneRef.GetCompactPoseIndex(BoneContainer));
FTransform PreviousBoneTransform = MeshBases.GetComponentSpaceTransform(BoundBoneReferences.Last().GetCompactPoseIndex(BoneContainer));
FVector PreviousAnchor = PreviousBoneTransform.TransformPosition(-LocalJointOffset);
float DistanceToAnchor = (PreviousBoneTransform.GetTranslation() - CurrentBoneTransform.GetTranslation()).Size() * 0.5f;
if(LocalJointOffset.SizeSquared() < SMALL_NUMBER)
{
// No offset, just use the position between chain links as the offset
// This is likely to just look horrible, but at least the bodies will
// be placed correctly and not stack up at the top of the chain.
JointOffsets.Add(PreviousAnchor - CurrentBoneTransform.GetTranslation());
}
else
{
// Extend offset along chain.
JointOffsets.Add(LocalJointOffset.GetSafeNormal() * DistanceToAnchor);
}
}
else
{
// No chain to worry about, just use the specified offset.
JointOffsets.Add(LocalJointOffset);
}
BoundBoneReferences.Add(LinkBoneRef);
FTransform BodyTransform = MeshBases.GetComponentSpaceTransform(LinkBoneRef.GetCompactPoseIndex(BoneContainer));
BodyTransform.SetTranslation(BodyTransform.GetTranslation() + BodyTransform.GetRotation().RotateVector(-LocalJointOffset));
FAnimPhysLinkedBody NewChainBody(BodyShapes, BodyTransform.GetTranslation(), LinkBoneRef);
FAnimPhysRigidBody& PhysicsBody = NewChainBody.RigidBody.PhysBody;
PhysicsBody.Pose.Orientation = BodyTransform.GetRotation();
PhysicsBody.PreviousOrientation = PhysicsBody.Pose.Orientation;
PhysicsBody.NextOrientation = PhysicsBody.Pose.Orientation;
PhysicsBody.CollisionType = CollisionType;
switch(PhysicsBody.CollisionType)
{
case AnimPhysCollisionType::CustomSphere:
PhysicsBody.SphereCollisionRadius = SphereCollisionRadius;
break;
case AnimPhysCollisionType::InnerSphere:
PhysicsBody.SphereCollisionRadius = BoxExtents.GetAbsMin() / 2.0f;
break;
case AnimPhysCollisionType::OuterSphere:
PhysicsBody.SphereCollisionRadius = BoxExtents.GetAbsMax() / 2.0f;
break;
default:
break;
}
if (bOverrideLinearDamping)
{
PhysicsBody.bLinearDampingOverriden = true;
PhysicsBody.LinearDamping = LinearDampingOverride;
}
if (bOverrideAngularDamping)
{
PhysicsBody.bAngularDampingOverriden = true;
PhysicsBody.AngularDamping = AngularDampingOverride;
}
PhysicsBody.GravityScale = GravityScale;
// Link to parent
if (Bodies.Num() > 0)
{
NewChainBody.ParentBody = &Bodies.Last().RigidBody;
}
Bodies.Add(NewChainBody);
}
BaseBodyPtrs.Empty();
for(FAnimPhysLinkedBody& Body : Bodies)
{
BaseBodyPtrs.Add(&Body.RigidBody.PhysBody);
}
// Set up transient constraint data
const bool bXAxisLocked = ConstraintSetup.LinearXLimitType != AnimPhysLinearConstraintType::Free && ConstraintSetup.LinearAxesMin.X - ConstraintSetup.LinearAxesMax.X == 0.0f;
const bool bYAxisLocked = ConstraintSetup.LinearYLimitType != AnimPhysLinearConstraintType::Free && ConstraintSetup.LinearAxesMin.Y - ConstraintSetup.LinearAxesMax.Y == 0.0f;
const bool bZAxisLocked = ConstraintSetup.LinearZLimitType != AnimPhysLinearConstraintType::Free && ConstraintSetup.LinearAxesMin.Z - ConstraintSetup.LinearAxesMax.Z == 0.0f;
ConstraintSetup.bLinearFullyLocked = bXAxisLocked && bYAxisLocked && bZAxisLocked;
// Cache physics settings to avoid accessing UPhysicsSettings continuously
if(UPhysicsSettings* Settings = UPhysicsSettings::Get())
{
MaxPhysicsDeltaTime = Settings->MaxPhysicsDeltaTime;
MaxSubstepDeltaTime = Settings->MaxSubstepDeltaTime;
MaxSubsteps = Settings->MaxSubsteps;
}
else
{
MaxPhysicsDeltaTime = (1.0f/30.0f);
MaxSubstepDeltaTime = (1.0f/60.0f);
MaxSubsteps = 4;
}
bRequiresInit = false;
}
void FAnimNode_TwoBoneIK::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, FCSPose<FCompactPose>& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
check(OutBoneTransforms.Num() == 0);
const FBoneContainer& BoneContainer = MeshBases.GetPose().GetBoneContainer();
// Get indices of the lower and upper limb bones and check validity.
bool bInvalidLimb = false;
FCompactPoseBoneIndex IKBoneCompactPoseIndex = IKBone.GetCompactPoseIndex(BoneContainer);
const FCompactPoseBoneIndex LowerLimbIndex = BoneContainer.GetParentBoneIndex(IKBoneCompactPoseIndex);
if (LowerLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
const FCompactPoseBoneIndex UpperLimbIndex = BoneContainer.GetParentBoneIndex(LowerLimbIndex);
if (UpperLimbIndex == INDEX_NONE)
{
bInvalidLimb = true;
}
const bool bInBoneSpace = (EffectorLocationSpace == BCS_ParentBoneSpace) || (EffectorLocationSpace == BCS_BoneSpace);
const int32 EffectorBoneIndex = bInBoneSpace ? BoneContainer.GetPoseBoneIndexForBoneName(EffectorSpaceBoneName) : INDEX_NONE;
const FCompactPoseBoneIndex EffectorSpaceBoneIndex = BoneContainer.MakeCompactPoseIndex(FMeshPoseBoneIndex(EffectorBoneIndex));
if (bInBoneSpace && (EffectorSpaceBoneIndex == INDEX_NONE))
{
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(IKBoneCompactPoseIndex);
// Now get those in component space...
FTransform LowerLimbCSTransform = MeshBases.GetComponentSpaceTransform(LowerLimbIndex);
FTransform UpperLimbCSTransform = MeshBases.GetComponentSpaceTransform(UpperLimbIndex);
FTransform EndBoneCSTransform = MeshBases.GetComponentSpaceTransform(IKBoneCompactPoseIndex);
// 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);
FCompactPoseBoneIndex JointTargetSpaceBoneIndex(INDEX_NONE);
if (JointTargetLocationSpace == BCS_ParentBoneSpace || JointTargetLocationSpace == BCS_BoneSpace)
{
int32 Index = BoneContainer.GetPoseBoneIndexForBoneName(JointTargetSpaceBoneName);
JointTargetSpaceBoneIndex = BoneContainer.MakeCompactPoseIndex(FMeshPoseBoneIndex(Index));
}
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).GetSafeNormal();
FVector const NewDir = (OutJointPos - RootPos).GetSafeNormal();
// Find Delta Rotation take takes us from Old to New dir
FQuat const DeltaRotation = FQuat::FindBetweenNormals(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::FindBetweenNormals(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(IKBoneCompactPoseIndex, EndBoneCSTransform));
}
// Make sure we have correct number of bones
check(OutBoneTransforms.Num() == 3);
}
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) );
}