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_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 UPoseableMeshComponent::SetBoneTransformByName(FName BoneName, const FTransform& InTransform, EBoneSpaces::Type BoneSpace)
{
	if( !SkeletalMesh || !RequiredBones.IsValid() )
	{
		return;
	}

	int32 BoneIndex = GetBoneIndex(BoneName);
	if(BoneIndex >=0 && BoneIndex < LocalAtoms.Num())
	{
		LocalAtoms[BoneIndex] = InTransform;

		// If we haven't requested local space we need to transform the position passed in
		//if(BoneSpace != EBoneSpaces::LocalSpace)
		{
			if(BoneSpace == EBoneSpaces::WorldSpace)
			{
				LocalAtoms[BoneIndex].SetToRelativeTransform(GetComponentToWorld());
			}

			int32 ParentIndex = RequiredBones.GetParentBoneIndex(BoneIndex);
			if(ParentIndex >=0)
			{
				FA2CSPose CSPose;
				CSPose.AllocateLocalPoses(RequiredBones, LocalAtoms);

				LocalAtoms[BoneIndex].SetToRelativeTransform(CSPose.GetComponentSpaceTransform(ParentIndex));
			}

			// Need to send new state to render thread
			MarkRenderDynamicDataDirty();
		}
	}
}
FTransform UPoseableMeshComponent::GetBoneTransformByName(FName BoneName, EBoneSpaces::Type BoneSpace)
{
	if( !SkeletalMesh || !RequiredBones.IsValid() )
	{
		return FTransform();
	}

	int32 BoneIndex = GetBoneIndex(BoneName);
	if( BoneIndex == INDEX_NONE)
	{
		FString Message = FString::Printf(TEXT("Invalid Bone Name '%s'"), *BoneName.ToString());
		FFrame::KismetExecutionMessage(*Message, ELogVerbosity::Warning);
		return FTransform();
	}

	/*if(BoneSpace == EBoneSpaces::LocalSpace)
	{
		return LocalAtoms[i];
	}*/

	FA2CSPose CSPose;
	CSPose.AllocateLocalPoses(RequiredBones, LocalAtoms);

	if(BoneSpace == EBoneSpaces::ComponentSpace)
	{
		return CSPose.GetComponentSpaceTransform(BoneIndex);
	}
	else
	{
		return CSPose.GetComponentSpaceTransform(BoneIndex) * ComponentToWorld;
	}
}
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));
			}
		}
	}
}
FQuat UAnimGraphNode_SkeletalControlBase::ConvertCSRotationToBoneSpace(const USkeletalMeshComponent* SkelComp, FRotator& InCSRotator, FA2CSPose& MeshBases, const FName& BoneName, const EBoneControlSpace Space)
{
	FQuat OutQuat = FQuat::Identity;

	if (MeshBases.IsValid())
	{
		int32 MeshBoneIndex = SkelComp->GetBoneIndex(BoneName);

		FVector RotAxis;
		float RotAngle;
		InCSRotator.Quaternion().ToAxisAndAngle(RotAxis, RotAngle);

		switch (Space)
		{
			// World Space, no change in preview window
		case BCS_WorldSpace:
		case BCS_ComponentSpace:
			// Component Space, no change.
			OutQuat = InCSRotator.Quaternion();
			break;

		case BCS_ParentBoneSpace:
		{
			const int32 ParentIndex = MeshBases.GetParentBoneIndex(MeshBoneIndex);
			if (ParentIndex != INDEX_NONE)
			{
				FTransform ParentTM = MeshBases.GetComponentSpaceTransform(ParentIndex);
				ParentTM = ParentTM.Inverse();
				//Calculate the new delta rotation
				FVector4 BoneSpaceAxis = ParentTM.TransformVector(RotAxis);
				FQuat DeltaQuat(BoneSpaceAxis, RotAngle);
				DeltaQuat.Normalize();
				OutQuat = DeltaQuat;
			}
		}
			break;

		case BCS_BoneSpace:
		{
			FTransform BoneTM = MeshBases.GetComponentSpaceTransform(MeshBoneIndex);
			BoneTM = BoneTM.Inverse();
			FVector4 BoneSpaceAxis = BoneTM.TransformVector(RotAxis);
			//Calculate the new delta rotation
			FQuat DeltaQuat(BoneSpaceAxis, RotAngle);
			DeltaQuat.Normalize();
			OutQuat = DeltaQuat;
		}
			break;
		}
	}

	return OutQuat;
}
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) );
        }
    }
}
FVector UAnimGraphNode_SkeletalControlBase::ConvertWidgetLocation(const USkeletalMeshComponent* SkelComp, FA2CSPose& MeshBases, const FName& BoneName, const FVector& Location, const EBoneControlSpace Space)
{
	FVector WidgetLoc = FVector::ZeroVector;

	if (MeshBases.IsValid())
	{
		USkeleton * Skeleton = SkelComp->SkeletalMesh->Skeleton;
		int32 MeshBoneIndex = SkelComp->GetBoneIndex(BoneName);

		switch (Space)
		{
			// ComponentToWorld must be Identity in preview window so same as ComponentSpace
		case BCS_WorldSpace:
		case BCS_ComponentSpace:
		{
			// Component Space, no change.
			WidgetLoc = Location;
		}
			break;

		case BCS_ParentBoneSpace:

			if (MeshBoneIndex != INDEX_NONE)
			{
				const int32 MeshParentIndex = MeshBases.GetParentBoneIndex(MeshBoneIndex);
				if (MeshParentIndex != INDEX_NONE)
				{
					const FTransform ParentTM = MeshBases.GetComponentSpaceTransform(MeshParentIndex);
					WidgetLoc = ParentTM.TransformPosition(Location);
				}
			}
			break;

		case BCS_BoneSpace:

			if (MeshBoneIndex != INDEX_NONE)
			{
				FTransform BoneTM = MeshBases.GetComponentSpaceTransform(MeshBoneIndex);
				WidgetLoc = BoneTM.TransformPosition(Location);
			}
		}
	}

	return WidgetLoc;
}
Esempio n. 9
0
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) );
}
FVector UAnimGraphNode_SkeletalControlBase::ConvertCSVectorToBoneSpace(const USkeletalMeshComponent* SkelComp, FVector& InCSVector, FA2CSPose& MeshBases, const FName& BoneName, const EBoneControlSpace Space)
{
	FVector OutVector = InCSVector;

	if (MeshBases.IsValid())
	{
		int32 MeshBoneIndex = SkelComp->GetBoneIndex(BoneName);

		switch (Space)
		{
			// World Space, no change in preview window
		case BCS_WorldSpace:
		case BCS_ComponentSpace:
			// Component Space, no change.
			break;

		case BCS_ParentBoneSpace:
		{
			const int32 ParentIndex = MeshBases.GetParentBoneIndex(MeshBoneIndex);
			if (ParentIndex != INDEX_NONE)
			{
				FTransform ParentTM = MeshBases.GetComponentSpaceTransform(ParentIndex);
				OutVector = ParentTM.InverseTransformVector(InCSVector);
			}
		}
			break;

		case BCS_BoneSpace:
		{
			FTransform BoneTM = MeshBases.GetComponentSpaceTransform(MeshBoneIndex);
			OutVector = BoneTM.InverseTransformVector(InCSVector);
		}
			break;
		}
	}

	return OutVector;
}
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;
}
Esempio n. 12
0
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();
}
Esempio n. 13
0
void FAnimationRuntime::SetSpaceTransform(FA2CSPose& Pose, int32 Index, FTransform& NewTransform)
{
	Pose.SetComponentSpaceTransform(Index, NewTransform);
}
Esempio n. 14
0
FTransform FAnimationRuntime::GetSpaceTransform(FA2CSPose& Pose, int32 Index)
{
	return Pose.GetComponentSpaceTransform(Index);
}
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;



		

	}


}
Esempio n. 16
0
FVector FAnimNode_Fabrik::GetCurrentLocation(FA2CSPose & MeshBases, int32 const & BoneIndex)
{
	return MeshBases.GetComponentSpaceTransform(BoneIndex).GetLocation();
}
void UDebugSkelMeshComponent::RefreshBoneTransforms(FActorComponentTickFunction* TickFunction)
{
	// Run regular update first so we get RequiredBones up to date.
	Super::RefreshBoneTransforms(NULL); // Pass NULL so we force non threaded work

	// Non retargeted pose.
	NonRetargetedSpaceBases.Empty();
	if( bDisplayNonRetargetedPose && AnimScriptInstance && AnimScriptInstance->RequiredBones.IsValid() )
	{
		TArray<FTransform> BackupSpaceBases = SpaceBases;

		AnimScriptInstance->RequiredBones.SetDisableRetargeting(true);
		Super::RefreshBoneTransforms(NULL);
		AnimScriptInstance->RequiredBones.SetDisableRetargeting(false);

		NonRetargetedSpaceBases = SpaceBases;
		SpaceBases = BackupSpaceBases;
	}

	if( bDisplayRawAnimation )
	{
		// save the transform in CompressedSpaceBases
		CompressedSpaceBases = SpaceBases;

		// use raw data now
		if( AnimScriptInstance && AnimScriptInstance->RequiredBones.IsValid() )
		{
			AnimScriptInstance->RequiredBones.SetUseRAWData(true);
			Super::RefreshBoneTransforms(NULL);
			AnimScriptInstance->RequiredBones.SetUseRAWData(false);
		}
		// Otherwise we'll just get ref pose.
		else
		{
			Super::RefreshBoneTransforms(NULL);
		}
	}
	else
	{
		CompressedSpaceBases.Empty();
	}

	const bool bIsPreviewInstance = (PreviewInstance && PreviewInstance == AnimScriptInstance);

	// Only works in PreviewInstance, and not for anim blueprint. This is intended.
	AdditiveBasePoses.Empty();
	if( bDisplayAdditiveBasePose && bIsPreviewInstance && PreviewInstance->RequiredBones.IsValid() )
	{
		if (UAnimSequence* Sequence = Cast<UAnimSequence>(PreviewInstance->CurrentAsset)) 
		{ 
			if (Sequence->IsValidAdditive()) 
			{ 
				AdditiveBasePoses.AddUninitialized(PreviewInstance->RequiredBones.GetNumBones());
				Sequence->GetAdditiveBasePose(AdditiveBasePoses, PreviewInstance->RequiredBones, FAnimExtractContext(PreviewInstance->CurrentTime));
				
				FA2CSPose CSPose;
				CSPose.AllocateLocalPoses(AnimScriptInstance->RequiredBones, AdditiveBasePoses);
				for(int32 i=0; i<AdditiveBasePoses.Num(); ++i)
				{
					AdditiveBasePoses[i] = CSPose.GetComponentSpaceTransform(i);
				}
			}
		}
	}
}
Esempio n. 18
0
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;
	}
}
Esempio n. 19
0
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_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_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("左つま先IK")) || EffectorSpaceBoneName.IsEqual(TEXT("右つま先IK")))
		{
			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();
			}

		}

	}

}