FQuat FAnimNode_RotationMultiplier::MultiplyQuatBasedOnSourceIndex(const FTransform& RefPoseTransform, const FTransform& LocalBoneTransform, const EBoneAxis Axis, float InMultiplier, const FQuat& ReferenceQuat)
{
// Find delta angle for source bone.
FQuat DeltaQuat = ExtractAngle(RefPoseTransform, LocalBoneTransform, Axis);
// Turn to Axis and Angle
FVector RotationAxis;
float RotationAngle;
DeltaQuat.ToAxisAndAngle(RotationAxis, RotationAngle);
const FVector DefaultAxis = GetAxisVector(Axis);
// See if we need to invert angle - shortest path
if( (RotationAxis | DefaultAxis) < 0.f )
{
RotationAxis = -RotationAxis;
RotationAngle = -RotationAngle;
}
// Make sure it is the shortest angle.
RotationAngle = FMath::UnwindRadians(RotationAngle);
// New bone rotation
FQuat OutQuat = ReferenceQuat * FQuat(RotationAxis, RotationAngle* InMultiplier);
// Normalize resulting quaternion.
OutQuat.Normalize();
#if 0 //DEBUG_TWISTBONECONTROLLER
UE_LOG(LogSkeletalControl, Log, TEXT("\t RefQuat: %s, Rot: %s"), *ReferenceQuat.ToString(), *ReferenceQuat.Rotator().ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t NewQuat: %s, Rot: %s"), *OutQuat.ToString(), *OutQuat.Rotator().ToString() );
UE_LOG(LogSkeletalControl, Log, TEXT("\t RollAxis: %s, RollAngle: %f"), *RotationAxis.ToString(), RotationAngle );
#endif
return OutQuat;
}
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;
}