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; }
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; }
void FAnimNode_Trail::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms) { check(OutBoneTransforms.Num() == 0); if( ChainLength < 2 ) { return; } // The incoming BoneIndex is the 'end' of the spline chain. We need to find the 'start' by walking SplineLength bones up hierarchy. // Fail if we walk past the root bone. int32 WalkBoneIndex = TrailBone.BoneIndex; TArray<int32> ChainBoneIndices; ChainBoneIndices.AddZeroed(ChainLength); ChainBoneIndices[ChainLength - 1] = WalkBoneIndex; for (int32 i = 1; i < ChainLength; i++) { // returns to avoid a crash // @TODO : shows an error message why failed if (WalkBoneIndex == 0) { return; } // Get parent bone. WalkBoneIndex = RequiredBones.GetParentBoneIndex(WalkBoneIndex); //Insert indices at the start of array, so that parents are before children in the array. int32 TransformIndex = ChainLength - (i + 1); ChainBoneIndices[TransformIndex] = WalkBoneIndex; } OutBoneTransforms.AddZeroed(ChainLength); // If we have >0 this frame, but didn't last time, record positions of all the bones. // Also do this if number has changed or array is zero. bool bHasValidStrength = (Alpha > 0.f); if(TrailBoneLocations.Num() != ChainLength || (bHasValidStrength && !bHadValidStrength)) { TrailBoneLocations.Empty(); TrailBoneLocations.AddZeroed(ChainLength); for(int32 i=0; i<ChainBoneIndices.Num(); i++) { int32 ChildIndex = ChainBoneIndices[i]; FTransform ChainTransform = MeshBases.GetComponentSpaceTransform(ChildIndex); TrailBoneLocations[i] = ChainTransform.GetTranslation(); } OldLocalToWorld = SkelComp->GetTransformMatrix(); } bHadValidStrength = bHasValidStrength; // transform between last frame and now. FMatrix OldToNewTM = OldLocalToWorld * SkelComp->GetTransformMatrix().InverseFast(); // Add fake velocity if present to all but root bone if(!FakeVelocity.IsZero()) { FVector FakeMovement = -FakeVelocity * ThisTimstep; if (bActorSpaceFakeVel && SkelComp->GetOwner()) { const FTransform BoneToWorld(SkelComp->GetOwner()->GetActorRotation(), SkelComp->GetOwner()->GetActorLocation()); FakeMovement = BoneToWorld.TransformVector(FakeMovement); } FakeMovement = SkelComp->GetTransformMatrix().InverseTransformVector(FakeMovement); // Then add to each bone for(int32 i=1; i<TrailBoneLocations.Num(); i++) { TrailBoneLocations[i] += FakeMovement; } } // Root bone of trail is not modified. int32 RootIndex = ChainBoneIndices[0]; FTransform ChainTransform = MeshBases.GetComponentSpaceTransform(RootIndex); OutBoneTransforms[0] = FBoneTransform(RootIndex, ChainTransform); TrailBoneLocations[0] = ChainTransform.GetTranslation(); // Starting one below head of chain, move bones. for(int32 i=1; i<ChainBoneIndices.Num(); i++) { // Parent bone position in component space. int32 ParentIndex = ChainBoneIndices[i-1]; FVector ParentPos = TrailBoneLocations[i-1]; FVector ParentAnimPos = MeshBases.GetComponentSpaceTransform(ParentIndex).GetTranslation(); // Child bone position in component space. int32 ChildIndex = ChainBoneIndices[i]; FVector ChildPos = OldToNewTM.TransformPosition(TrailBoneLocations[i]); // move from 'last frames component' frame to 'this frames component' frame FVector ChildAnimPos = MeshBases.GetComponentSpaceTransform(ChildIndex).GetTranslation(); // Desired parent->child offset. FVector TargetDelta = (ChildAnimPos - ParentAnimPos); // Desired child position. FVector ChildTarget = ParentPos + TargetDelta; // Find vector from child to target FVector Error = ChildTarget - ChildPos; // Calculate how much to push the child towards its target float Correction = FMath::Clamp<float>(ThisTimstep * TrailRelaxation, 0.f, 1.f); // Scale correction vector and apply to get new world-space child position. TrailBoneLocations[i] = ChildPos + (Error * Correction); // If desired, prevent bones stretching too far. if(bLimitStretch) { float RefPoseLength = TargetDelta.Size(); FVector CurrentDelta = TrailBoneLocations[i] - TrailBoneLocations[i-1]; float CurrentLength = CurrentDelta.Size(); // If we are too far - cut it back (just project towards parent particle). if( (CurrentLength - RefPoseLength > StretchLimit) && CurrentLength > SMALL_NUMBER ) { FVector CurrentDir = CurrentDelta / CurrentLength; TrailBoneLocations[i] = TrailBoneLocations[i-1] + (CurrentDir * (RefPoseLength + StretchLimit)); } } // Modify child matrix OutBoneTransforms[i] = FBoneTransform(ChildIndex, MeshBases.GetComponentSpaceTransform(ChildIndex)); OutBoneTransforms[i].Transform.SetTranslation(TrailBoneLocations[i]); // Modify rotation of parent matrix to point at this one. // Calculate the direction that parent bone is currently pointing. FVector CurrentBoneDir = OutBoneTransforms[i-1].Transform.TransformVector( GetAlignVector(ChainBoneAxis, bInvertChainBoneAxis) ); CurrentBoneDir = CurrentBoneDir.SafeNormal(SMALL_NUMBER); // Calculate vector from parent to child. FVector NewBoneDir = FVector(OutBoneTransforms[i].Transform.GetTranslation() - OutBoneTransforms[i - 1].Transform.GetTranslation()).SafeNormal(SMALL_NUMBER); // Calculate a quaternion that gets us from our current rotation to the desired one. FQuat DeltaLookQuat = FQuat::FindBetween(CurrentBoneDir, NewBoneDir); FTransform DeltaTM( DeltaLookQuat, FVector(0.f) ); // Apply to the current parent bone transform. FTransform TmpMatrix = FTransform::Identity; TmpMatrix.CopyRotationPart(OutBoneTransforms[i - 1].Transform); TmpMatrix = TmpMatrix * DeltaTM; OutBoneTransforms[i - 1].Transform.CopyRotationPart(TmpMatrix); } // For the last bone in the chain, use the rotation from the bone above it. OutBoneTransforms[ChainLength - 1].Transform.CopyRotationPart(OutBoneTransforms[ChainLength - 2].Transform); // Update OldLocalToWorld OldLocalToWorld = SkelComp->GetTransformMatrix(); }
void FAnimationRuntime::SetSpaceTransform(FA2CSPose& Pose, int32 Index, FTransform& NewTransform) { Pose.SetComponentSpaceTransform(Index, NewTransform); }
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; } }
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); } } } } }
void FAnimNode_Fabrik::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer& RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms) { // IsValidToEvaluate validated our inputs, we don't need to check pre-requisites again. int32 const RootIndex = RootBone.BoneIndex; // Update EffectorLocation if it is based off a bone position FTransform CSEffectorTransform = FTransform(EffectorTransform); FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, CSEffectorTransform, EffectorTransformBone.BoneIndex, EffectorTransformSpace); FVector const CSEffectorLocation = CSEffectorTransform.GetLocation(); // @fixme - we need better to draw widgets and debug information in editor. #if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) if (bEnableDebugDraw) { // Show end effector position. DrawDebugBox(SkelComp->GetWorld(), CSEffectorLocation, FVector(Precision), FColor::Green, true, 0.1f); DrawDebugCoordinateSystem(SkelComp->GetWorld(), CSEffectorLocation, CSEffectorTransform.GetRotation().Rotator(), 5.f, true, 0.1f); } #endif // Gather all bone indices between root and tip. TArray<int32> BoneIndices; { int32 BoneIndex = TipBone.BoneIndex; do { BoneIndices.Insert(BoneIndex, 0); BoneIndex = RequiredBones.GetParentBoneIndex(BoneIndex); } while (BoneIndex != RootIndex); BoneIndices.Insert(BoneIndex, 0); } // Maximum length of skeleton segment at full extension float MaximumReach = 0; // Gather transforms int32 const NumTransforms = BoneIndices.Num(); OutBoneTransforms.AddUninitialized(NumTransforms); // Gather chain links. These are non zero length bones. TArray<FABRIKChainLink> Chain; Chain.Reserve(NumTransforms); // Start with Root Bone { int32 const & RootBoneIndex = BoneIndices[0]; FTransform const BoneCSTransform = MeshBases.GetComponentSpaceTransform(RootBoneIndex); OutBoneTransforms[0] = FBoneTransform(RootBoneIndex, BoneCSTransform); Chain.Add(FABRIKChainLink(BoneCSTransform.GetLocation(), 0.f, RootBoneIndex, 0)); } // Go through remaining transforms for (int32 TransformIndex = 1; TransformIndex < NumTransforms; TransformIndex++) { int32 const & BoneIndex = BoneIndices[TransformIndex]; FTransform const BoneCSTransform = MeshBases.GetComponentSpaceTransform(BoneIndex); FVector const BoneCSPosition = BoneCSTransform.GetLocation(); OutBoneTransforms[TransformIndex] = FBoneTransform(BoneIndex, BoneCSTransform); // Calculate the combined length of this segment of skeleton float const BoneLength = FVector::Dist(BoneCSPosition, OutBoneTransforms[TransformIndex-1].Transform.GetLocation()); if (!FMath::IsNearlyZero(BoneLength)) { Chain.Add(FABRIKChainLink(BoneCSPosition, BoneLength, BoneIndex, TransformIndex)); MaximumReach += BoneLength; } else { // Mark this transform as a zero length child of the last link. // It will inherit position and delta rotation from parent link. FABRIKChainLink & ParentLink = Chain[Chain.Num()-1]; ParentLink.ChildZeroLengthTransformIndices.Add(TransformIndex); } } bool bBoneLocationUpdated = false; float const RootToTargetDist = FVector::Dist(Chain[0].Position, CSEffectorLocation); int32 const NumChainLinks = Chain.Num(); // FABRIK algorithm - bone translation calculation // If the effector is further away than the distance from root to tip, simply move all bones in a line from root to effector location if (RootToTargetDist > MaximumReach) { for (int32 LinkIndex = 1; LinkIndex < NumChainLinks; LinkIndex++) { FABRIKChainLink const & ParentLink = Chain[LinkIndex - 1]; FABRIKChainLink & CurrentLink = Chain[LinkIndex]; CurrentLink.Position = ParentLink.Position + (CSEffectorLocation - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length; } bBoneLocationUpdated = true; } else // Effector is within reach, calculate bone translations to position tip at effector location { int32 const TipBoneLinkIndex = NumChainLinks - 1; // Check distance between tip location and effector location float Slop = FVector::Dist(Chain[TipBoneLinkIndex].Position, CSEffectorLocation); if (Slop > Precision) { // Set tip bone at end effector location. Chain[TipBoneLinkIndex].Position = CSEffectorLocation; int32 IterationCount = 0; while ((Slop > Precision) && (IterationCount++ < MaxIterations)) { // "Forward Reaching" stage - adjust bones from end effector. for (int32 LinkIndex = TipBoneLinkIndex - 1; LinkIndex > 0; LinkIndex--) { FABRIKChainLink & CurrentLink = Chain[LinkIndex]; FABRIKChainLink const & ChildLink = Chain[LinkIndex + 1]; CurrentLink.Position = ChildLink.Position + (CurrentLink.Position - ChildLink.Position).GetUnsafeNormal() * ChildLink.Length; } // "Backward Reaching" stage - adjust bones from root. for (int32 LinkIndex = 1; LinkIndex < TipBoneLinkIndex; LinkIndex++) { FABRIKChainLink const & ParentLink = Chain[LinkIndex - 1]; FABRIKChainLink & CurrentLink = Chain[LinkIndex]; CurrentLink.Position = ParentLink.Position + (CurrentLink.Position - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length; } // Re-check distance between tip location and effector location // Since we're keeping tip on top of effector location, check with its parent bone. Slop = FMath::Abs(Chain[TipBoneLinkIndex].Length - FVector::Dist(Chain[TipBoneLinkIndex - 1].Position, CSEffectorLocation)); } // Place tip bone based on how close we got to target. { FABRIKChainLink const & ParentLink = Chain[TipBoneLinkIndex - 1]; FABRIKChainLink & CurrentLink = Chain[TipBoneLinkIndex]; CurrentLink.Position = ParentLink.Position + (CurrentLink.Position - ParentLink.Position).GetUnsafeNormal() * CurrentLink.Length; } bBoneLocationUpdated = true; } } // If we moved some bones, update bone transforms. if (bBoneLocationUpdated) { // First step: update bone transform positions from chain links. for (int32 LinkIndex = 0; LinkIndex < NumChainLinks; LinkIndex++) { FABRIKChainLink const & ChainLink = Chain[LinkIndex]; OutBoneTransforms[ChainLink.TransformIndex].Transform.SetTranslation(ChainLink.Position); // If there are any zero length children, update position of those int32 const NumChildren = ChainLink.ChildZeroLengthTransformIndices.Num(); for (int32 ChildIndex = 0; ChildIndex < NumChildren; ChildIndex++) { OutBoneTransforms[ChainLink.ChildZeroLengthTransformIndices[ChildIndex]].Transform.SetTranslation(ChainLink.Position); } } // FABRIK algorithm - re-orientation of bone local axes after translation calculation for (int32 LinkIndex = 0; LinkIndex < NumChainLinks - 1; LinkIndex++) { FABRIKChainLink const & CurrentLink = Chain[LinkIndex]; FABRIKChainLink const & ChildLink = Chain[LinkIndex + 1]; // Calculate pre-translation vector between this bone and child FVector const OldDir = (GetCurrentLocation(MeshBases, ChildLink.BoneIndex) - GetCurrentLocation(MeshBases, CurrentLink.BoneIndex)).GetUnsafeNormal(); // Get vector from the post-translation bone to it's child FVector const NewDir = (ChildLink.Position - CurrentLink.Position).GetUnsafeNormal(); // Calculate axis of rotation from pre-translation vector to post-translation vector FVector const RotationAxis = FVector::CrossProduct(OldDir, NewDir).GetSafeNormal(); float const RotationAngle = FMath::Acos(FVector::DotProduct(OldDir, NewDir)); FQuat const DeltaRotation = FQuat(RotationAxis, RotationAngle); // We're going to multiply it, in order to not have to re-normalize the final quaternion, it has to be a unit quaternion. checkSlow(DeltaRotation.IsNormalized()); // Calculate absolute rotation and set it FTransform& CurrentBoneTransform = OutBoneTransforms[CurrentLink.TransformIndex].Transform; CurrentBoneTransform.SetRotation(DeltaRotation * CurrentBoneTransform.GetRotation()); // Update zero length children if any int32 const NumChildren = CurrentLink.ChildZeroLengthTransformIndices.Num(); for (int32 ChildIndex = 0; ChildIndex < NumChildren; ChildIndex++) { FTransform& ChildBoneTransform = OutBoneTransforms[CurrentLink.ChildZeroLengthTransformIndices[ChildIndex]].Transform; ChildBoneTransform.SetRotation(DeltaRotation * ChildBoneTransform.GetRotation()); } } } // Special handling for tip bone's rotation. int32 const TipBoneTransformIndex = OutBoneTransforms.Num() - 1; switch (EffectorRotationSource) { case BRS_KeepLocalSpaceRotation: OutBoneTransforms[TipBoneTransformIndex].Transform = MeshBases.GetLocalSpaceTransform(BoneIndices[TipBoneTransformIndex]) * OutBoneTransforms[TipBoneTransformIndex - 1].Transform; break; case BRS_CopyFromTarget: OutBoneTransforms[TipBoneTransformIndex].Transform.SetRotation(CSEffectorTransform.GetRotation()); break; case BRS_KeepComponentSpaceRotation: // Don't change the orientation at all break; default: break; } }
void FAnimNode_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(); } } } }