FQuat FAnimNode_RotationMultiplier::ExtractAngle(const TArray<FTransform> & RefPoseTransforms, FA2CSPose & MeshBases, const EBoneAxis Axis, int32 SourceBoneIndex)
{
// local bone transform
const FTransform & LocalBoneTransform = MeshBases.GetLocalSpaceTransform(SourceBoneIndex);
// local bone transform with reference rotation
FTransform ReferenceBoneTransform = RefPoseTransforms[SourceBoneIndex];
ReferenceBoneTransform.SetTranslation(LocalBoneTransform.GetTranslation());
// find delta angle between the two quaternions X Axis.
const FVector RotationAxis = GetAxisVector(Axis);
const FVector LocalRotationVector = LocalBoneTransform.GetRotation().RotateVector(RotationAxis);
const FVector ReferenceRotationVector = ReferenceBoneTransform.GetRotation().RotateVector(RotationAxis);
const FQuat LocalToRefQuat = FQuat::FindBetween(LocalRotationVector, ReferenceRotationVector);
checkSlow( LocalToRefQuat.IsNormalized() );
// Rotate parent bone atom from position in local space to reference skeleton
// Since our rotation rotates both vectors with shortest arc
// we're essentially left with a quaternion that has angle difference with reference skeleton version
const FQuat BoneQuatAligned = LocalToRefQuat * LocalBoneTransform.GetRotation();
checkSlow( BoneQuatAligned.IsNormalized() );
// Find that delta angle
const FQuat DeltaQuat = (ReferenceBoneTransform.GetRotation().Inverse()) * BoneQuatAligned;
checkSlow( DeltaQuat.IsNormalized() );
#if 0 //DEBUG_TWISTBONECONTROLLER
UE_LOG(LogSkeletalControl, Log, TEXT("\t ExtractAngle, Bone: %s (%d)"),
*SourceBone.BoneName.ToString(), SourceBoneIndex);
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t Bone Quat: %s, Rot: %s, AxisX: %s"), *LocalBoneTransform.GetRotation().ToString(), *LocalBoneTransform.GetRotation().Rotator().ToString(), *LocalRotationVector.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t BoneRef Quat: %s, Rot: %s, AxisX: %s"), *ReferenceBoneTransform.GetRotation().ToString(), *ReferenceBoneTransform.GetRotation().Rotator().ToString(), *ReferenceRotationVector.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t LocalToRefQuat Quat: %s, Rot: %s"), *LocalToRefQuat.ToString(), *LocalToRefQuat.Rotator().ToString() );
const FVector BoneQuatAlignedX = LocalBoneTransform.GetRotation().RotateVector(RotationAxis);
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t BoneQuatAligned Quat: %s, Rot: %s, AxisX: %s"), *BoneQuatAligned.ToString(), *BoneQuatAligned.Rotator().ToString(), *BoneQuatAlignedX.ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaQuat Quat: %s, Rot: %s"), *DeltaQuat.ToString(), *DeltaQuat.Rotator().ToString() );
FTransform BoneAtomAligned(BoneQuatAligned, ReferenceBoneTransform.GetTranslation());
const FQuat DeltaQuatAligned = FQuat::FindBetween(BoneAtomAligned.GetScaledAxis( EAxis::X ), ReferenceBoneTransform.GetScaledAxis( EAxis::X ));
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaQuatAligned Quat: %s, Rot: %s"), *DeltaQuatAligned.ToString(), *DeltaQuatAligned.Rotator().ToString() );
FVector DeltaAxis;
float DeltaAngle;
DeltaQuat.ToAxisAndAngle(DeltaAxis, DeltaAngle);
UE_LOG(LogSkeletalControl, Log, TEXT("\t\t DeltaAxis: %s, DeltaAngle: %f"), *DeltaAxis.ToString(), DeltaAngle );
#endif
return DeltaQuat;
}
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;
}
}