void DrawDebugDirectionalArrow(const UWorld* InWorld, FVector const& LineStart, FVector const& LineEnd, float ArrowSize, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority) { // no debug line drawing on dedicated server if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer) { if (ArrowSize <= 0) { ArrowSize = 10.f; } DrawDebugLine(InWorld, LineStart, LineEnd, Color, bPersistentLines, LifeTime); FVector Dir = (LineEnd-LineStart); Dir.Normalize(); FVector Up(0, 0, 1); FVector Right = Dir ^ Up; if (!Right.IsNormalized()) { Dir.FindBestAxisVectors(Up, Right); } FVector Origin = FVector::ZeroVector; FMatrix TM; // get matrix with dir/right/up TM.SetAxes(&Dir, &Right, &Up, &Origin); // since dir is x direction, my arrow will be pointing +y, -x and -y, -x float ArrowSqrt = FMath::Sqrt(ArrowSize); FVector ArrowPos; DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority); DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, -ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority); } }
void UAnimGraphNode_BoneDrivenController::Draw(FPrimitiveDrawInterface* PDI, USkeletalMeshComponent* SkelMeshComp) const { static const float ArrowHeadWidth = 5.0f; static const float ArrowHeadHeight = 8.0f; const int32 SourceIdx = SkelMeshComp->GetBoneIndex(Node.SourceBone.BoneName); const int32 TargetIdx = SkelMeshComp->GetBoneIndex(Node.TargetBone.BoneName); if ((SourceIdx != INDEX_NONE) && (TargetIdx != INDEX_NONE)) { const FTransform SourceTM = SkelMeshComp->GetSpaceBases()[SourceIdx] * SkelMeshComp->ComponentToWorld; const FTransform TargetTM = SkelMeshComp->GetSpaceBases()[TargetIdx] * SkelMeshComp->ComponentToWorld; PDI->DrawLine(TargetTM.GetLocation(), SourceTM.GetLocation(), FLinearColor(0.0f, 0.0f, 1.0f), SDPG_Foreground, 0.5f); const FVector ToTarget = TargetTM.GetTranslation() - SourceTM.GetTranslation(); const FVector UnitToTarget = ToTarget.GetSafeNormal(); FVector Midpoint = SourceTM.GetTranslation() + 0.5f * ToTarget + 0.5f * UnitToTarget * ArrowHeadHeight; FVector YAxis; FVector ZAxis; UnitToTarget.FindBestAxisVectors(YAxis, ZAxis); const FMatrix ArrowMatrix(UnitToTarget, YAxis, ZAxis, Midpoint); DrawConnectedArrow(PDI, ArrowMatrix, FLinearColor(0.0f, 1.0f, 0.0), ArrowHeadHeight, ArrowHeadWidth, SDPG_Foreground); PDI->DrawPoint(SourceTM.GetTranslation(), FLinearColor(0.8f, 0.8f, 0.2f), 5.0f, SDPG_Foreground); PDI->DrawPoint(SourceTM.GetTranslation() + ToTarget, FLinearColor(0.8f, 0.8f, 0.2f), 5.0f, SDPG_Foreground); } }
/** * Draws a line with an arrow at the end. * * @param Start Starting point of the line. * @param End Ending point of the line. * @param Color Color of the line. * @param Mag Size of the arrow. */ void FDebugRenderSceneProxy::DrawLineArrow(FPrimitiveDrawInterface* PDI,const FVector &Start,const FVector &End,const FColor &Color,float Mag) const { // draw a pretty arrow FVector Dir = End - Start; const float DirMag = Dir.Size(); Dir /= DirMag; FVector YAxis, ZAxis; Dir.FindBestAxisVectors(YAxis,ZAxis); FMatrix ArrowTM(Dir,YAxis,ZAxis,Start); DrawDirectionalArrow(PDI,ArrowTM,Color,DirMag,Mag,SDPG_World); }
void DrawDebugCylinder(const UWorld* InWorld, FVector const& Start, FVector const& End, float Radius, int32 Segments, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority) { // no debug line drawing on dedicated server if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer) { // Need at least 4 segments Segments = FMath::Max(Segments, 4); // Rotate a point around axis to form cylinder segments FVector Segment; FVector P1, P2, P3, P4; const int32 AngleInc = 360.f / Segments; int32 Angle = AngleInc; // Default for Axis is up FVector Axis = (End - Start).SafeNormal(); if( Axis.IsZero() ) { Axis = FVector(0.f, 0.f, 1.f); } FVector Perpendicular; FVector Dummy; Axis.FindBestAxisVectors(Perpendicular, Dummy); Segment = Perpendicular.RotateAngleAxis(0, Axis) * Radius; P1 = Segment + Start; P3 = Segment + End; // this means foreground lines can't be persistent ULineBatchComponent* const LineBatcher = GetDebugLineBatcher( InWorld, bPersistentLines, LifeTime, (DepthPriority == SDPG_Foreground) ); if(LineBatcher != NULL) { while( Segments-- ) { Segment = Perpendicular.RotateAngleAxis(Angle, Axis) * Radius; P2 = Segment + Start; P4 = Segment + End; LineBatcher->DrawLine(P2, P4, Color, DepthPriority); LineBatcher->DrawLine(P1, P2, Color, DepthPriority); LineBatcher->DrawLine(P3, P4, Color, DepthPriority); P1 = P2; P3 = P4; Angle += AngleInc; } } } }
// // Build an FPoly representing an "infinite" plane (which exceeds the maximum // dimensions of the world in all directions) for a particular Bsp node. // FPoly FBSPOps::BuildInfiniteFPoly( UModel* Model, int32 iNode ) { FBspNode &Node = Model->Nodes [iNode ]; FBspSurf &Poly = Model->Surfs [Node.iSurf ]; FVector Base = Poly.Plane * Poly.Plane.W; FVector Normal = Poly.Plane; FVector Axis1,Axis2; // Find two non-problematic axis vectors. Normal.FindBestAxisVectors( Axis1, Axis2 ); // Set up the FPoly. FPoly EdPoly; EdPoly.Init(); EdPoly.Normal = Normal; EdPoly.Base = Base; new(EdPoly.Vertices) FVector(Base + Axis1*WORLD_MAX + Axis2*WORLD_MAX); new(EdPoly.Vertices) FVector(Base - Axis1*WORLD_MAX + Axis2*WORLD_MAX); new(EdPoly.Vertices) FVector(Base - Axis1*WORLD_MAX - Axis2*WORLD_MAX); new(EdPoly.Vertices) FVector(Base + Axis1*WORLD_MAX - Axis2*WORLD_MAX); return EdPoly; }
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 DrawDebugCone(const UWorld* InWorld, FVector const& Origin, FVector const& Direction, float Length, float AngleWidth, float AngleHeight, int32 NumSides, FColor const& DrawColor, bool bPersistentLines, float LifeTime, uint8 DepthPriority) { // no debug line drawing on dedicated server if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer) { // Need at least 4 sides NumSides = FMath::Max(NumSides, 4); const float Angle1 = FMath::Clamp<float>(AngleHeight, (float)KINDA_SMALL_NUMBER, (float)(PI - KINDA_SMALL_NUMBER)); const float Angle2 = FMath::Clamp<float>(AngleWidth, (float)KINDA_SMALL_NUMBER, (float)(PI - KINDA_SMALL_NUMBER)); const float SinX_2 = FMath::Sin(0.5f * Angle1); const float SinY_2 = FMath::Sin(0.5f * Angle2); const float SinSqX_2 = SinX_2 * SinX_2; const float SinSqY_2 = SinY_2 * SinY_2; const float TanX_2 = FMath::Tan(0.5f * Angle1); const float TanY_2 = FMath::Tan(0.5f * Angle2); TArray<FVector> ConeVerts; ConeVerts.AddUninitialized(NumSides); for(int32 i = 0; i < NumSides; i++) { const float Fraction = (float)i/(float)(NumSides); const float Thi = 2.f * PI * Fraction; const float Phi = FMath::Atan2(FMath::Sin(Thi)*SinY_2, FMath::Cos(Thi)*SinX_2); const float SinPhi = FMath::Sin(Phi); const float CosPhi = FMath::Cos(Phi); const float SinSqPhi = SinPhi*SinPhi; const float CosSqPhi = CosPhi*CosPhi; const float RSq = SinSqX_2*SinSqY_2 / (SinSqX_2*SinSqPhi + SinSqY_2*CosSqPhi); const float R = FMath::Sqrt(RSq); const float Sqr = FMath::Sqrt(1-RSq); const float Alpha = R*CosPhi; const float Beta = R*SinPhi; ConeVerts[i].X = (1 - 2*RSq); ConeVerts[i].Y = 2 * Sqr * Alpha; ConeVerts[i].Z = 2 * Sqr * Beta; } // Calculate transform for cone. FVector YAxis, ZAxis; FVector DirectionNorm = Direction.SafeNormal(); DirectionNorm.FindBestAxisVectors(YAxis, ZAxis); const FMatrix ConeToWorld = FScaleMatrix(FVector(Length)) * FMatrix(DirectionNorm, YAxis, ZAxis, Origin); // this means foreground lines can't be persistent ULineBatchComponent* const LineBatcher = GetDebugLineBatcher( InWorld, bPersistentLines, LifeTime, (DepthPriority == SDPG_Foreground) ); if(LineBatcher != NULL) { float const LineLifeTime = (LifeTime > 0.f) ? LifeTime : LineBatcher->DefaultLifeTime; TArray<FBatchedLine> Lines; Lines.Empty(NumSides); FVector CurrentPoint, PrevPoint, FirstPoint; for(int32 i = 0; i < NumSides; i++) { CurrentPoint = ConeToWorld.TransformPosition(ConeVerts[i]); Lines.Add(FBatchedLine(ConeToWorld.GetOrigin(), CurrentPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority)); // PrevPoint must be defined to draw junctions if( i > 0 ) { Lines.Add(FBatchedLine(PrevPoint, CurrentPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority)); } else { FirstPoint = CurrentPoint; } PrevPoint = CurrentPoint; } // Connect last junction to first Lines.Add(FBatchedLine(CurrentPoint, FirstPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority)); LineBatcher->DrawLines(Lines); } } }
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(); } } } }