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("左つま先ＩＫ")) || EffectorSpaceBoneName.IsEqual(TEXT("右つま先ＩＫ")))
{
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();
}
}
}
}
