void DrawDebugCamera(const UWorld* InWorld, FVector const& Location, FRotator const& Rotation, float FOVDeg, float Scale, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority)
{
static float BaseScale = 4.f;
static FVector BaseProportions(2.f, 1.f, 1.5f);
// no debug line drawing on dedicated server
if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
{
DrawDebugCoordinateSystem(InWorld, Location, Rotation, BaseScale*Scale, bPersistentLines, DepthPriority);
FVector Extents = BaseProportions * BaseScale * Scale;
DrawDebugBox(InWorld, Location, Extents, Rotation.Quaternion(), Color, bPersistentLines, LifeTime, DepthPriority); // lifetime
// draw "lens" portion
FRotationTranslationMatrix Axes(Rotation, Location);
FVector XAxis = Axes.GetScaledAxis( EAxis::X );
FVector YAxis = Axes.GetScaledAxis( EAxis::Y );
FVector ZAxis = Axes.GetScaledAxis( EAxis::Z );
FVector LensPoint = Location + XAxis * Extents.X;
float LensSize = BaseProportions.Z * Scale * BaseScale;
float HalfLensSize = LensSize * FMath::Tan(FMath::DegreesToRadians(FOVDeg*0.5f));
FVector Corners[4] =
{
LensPoint + XAxis * LensSize + (YAxis * HalfLensSize) + (ZAxis * HalfLensSize),
LensPoint + XAxis * LensSize + (YAxis * HalfLensSize) - (ZAxis * HalfLensSize),
LensPoint + XAxis * LensSize - (YAxis * HalfLensSize) - (ZAxis * HalfLensSize),
LensPoint + XAxis * LensSize - (YAxis * HalfLensSize) + (ZAxis * HalfLensSize),
};
DrawDebugLine(InWorld, LensPoint, Corners[0], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, LensPoint, Corners[1], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, LensPoint, Corners[2], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, LensPoint, Corners[3], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, Corners[0], Corners[1], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, Corners[1], Corners[2], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, Corners[2], Corners[3], Color, bPersistentLines, LifeTime, DepthPriority);
DrawDebugLine(InWorld, Corners[3], Corners[0], Color, bPersistentLines, LifeTime, DepthPriority);
}
}
FTransform UPaperTerrainComponent::GetTransformAtDistance(float InDistance) const
{
const float SplineLength = AssociatedSpline->GetSplineLength();
InDistance = FMath::Clamp<float>(InDistance, 0.0f, SplineLength);
const float Param = AssociatedSpline->SplineReparamTable.Eval(InDistance, 0.0f);
const FVector Position3D = AssociatedSpline->SplineInfo.Eval(Param, FVector::ZeroVector);
const FVector Tangent = AssociatedSpline->SplineInfo.EvalDerivative(Param, FVector(1.0f, 0.0f, 0.0f)).GetSafeNormal();
const FVector NormalEst = AssociatedSpline->SplineInfo.EvalSecondDerivative(Param, FVector(0.0f, 1.0f, 0.0f)).GetSafeNormal();
const FVector Bitangent = FVector::CrossProduct(Tangent, NormalEst);
const FVector Normal = FVector::CrossProduct(Bitangent, Tangent);
const FVector Floop = FVector::CrossProduct(PaperAxisZ, Tangent);
FTransform LocalTransform(Tangent, PaperAxisZ, Floop, Position3D);
LocalTransform = FTransform(FRotator(0.0f, 180.0f, 0.0f), FVector::ZeroVector) * LocalTransform;
#if PAPER_TERRAIN_DRAW_DEBUG
FTransform WorldTransform = LocalTransform * ComponentToWorld;
const float Time = 2.5f;
DrawDebugCoordinateSystem(GetWorld(), WorldTransform.GetLocation(), FRotator(WorldTransform.GetRotation()), 15.0f, true, Time, SDPG_Foreground);
// FVector WorldPos = ComponentToWorld.TransformPosition(Position3D);
// WorldPos.Y -= 0.01;
//
// //DrawDebugLine(GetWorld(), WorldPos, WorldPos + ComponentToWorld.TransformVector(Tangent) * 10.0f, FColor::Red, true, Time);
// // DrawDebugLine(GetWorld(), WorldPos, WorldPos + ComponentToWorld.TransformVector(NormalEst) * 10.0f, FColor::Green, true, Time);
// // DrawDebugLine(GetWorld(), WorldPos, WorldPos + ComponentToWorld.TransformVector(Bitangent) * 10.0f, FColor::Blue, true, Time);
// //DrawDebugLine(GetWorld(), WorldPos, WorldPos + ComponentToWorld.TransformVector(Floop) * 10.0f, FColor::Yellow, true, Time);
// // DrawDebugPoint(GetWorld(), WorldPos, 4.0f, FColor::White, true, 1.0f);
#endif
return LocalTransform;
}
void UPaperTerrainComponent::OnSplineEdited()
{
// Ensure we have the data structure for the desired collision method
if (SpriteCollisionDomain == ESpriteCollisionMode::Use3DPhysics)
{
CachedBodySetup = NewObject<UBodySetup>(this);
}
else
{
CachedBodySetup = nullptr;
}
const float SlopeAnalysisTimeRate = 10.0f;
const float FillRasterizationTimeRate = 100.0f;
GeneratedSpriteGeometry.Empty();
if ((AssociatedSpline != nullptr) && (TerrainMaterial != nullptr))
{
if (AssociatedSpline->ReparamStepsPerSegment != ReparamStepsPerSegment)
{
AssociatedSpline->ReparamStepsPerSegment = ReparamStepsPerSegment;
AssociatedSpline->UpdateSpline();
}
FRandomStream RandomStream(RandomSeed);
const FInterpCurveVector& SplineInfo = AssociatedSpline->SplineInfo;
float SplineLength = AssociatedSpline->GetSplineLength();
struct FTerrainRuleHelper
{
public:
FTerrainRuleHelper(const FPaperTerrainMaterialRule* Rule)
: StartWidth(0.0f)
, EndWidth(0.0f)
{
for (const UPaperSprite* Sprite : Rule->Body)
{
if (Sprite != nullptr)
{
const float Width = GetSpriteRenderDataBounds2D(Sprite->BakedRenderData).GetSize().X;
if (Width > 0.0f)
{
ValidBodies.Add(Sprite);
ValidBodyWidths.Add(Width);
}
}
}
if (Rule->StartCap != nullptr)
{
const float Width = GetSpriteRenderDataBounds2D(Rule->StartCap->BakedRenderData).GetSize().X;
if (Width > 0.0f)
{
StartWidth = Width;
}
}
if (Rule->EndCap != nullptr)
{
const float Width = GetSpriteRenderDataBounds2D(Rule->EndCap->BakedRenderData).GetSize().X;
if (Width > 0.0f)
{
EndWidth = Width;
}
}
}
float StartWidth;
float EndWidth;
TArray<const UPaperSprite*> ValidBodies;
TArray<float> ValidBodyWidths;
int32 GenerateBodyIndex(FRandomStream& InRandomStream) const
{
check(ValidBodies.Num() > 0);
return InRandomStream.GetUnsignedInt() % ValidBodies.Num();
}
};
// Split the spline into segments based on the slope rules in the material
TArray<FTerrainSegment> Segments;
FTerrainSegment* ActiveSegment = new (Segments) FTerrainSegment();
ActiveSegment->StartTime = 0.0f;
ActiveSegment->EndTime = SplineLength;
{
float CurrentTime = 0.0f;
while (CurrentTime < SplineLength)
{
const FTransform Frame(GetTransformAtDistance(CurrentTime));
const FVector UnitTangent = Frame.GetUnitAxis(EAxis::X);
const float RawSlopeAngleRadians = FMath::Atan2(FVector::DotProduct(UnitTangent, PaperAxisY), FVector::DotProduct(UnitTangent, PaperAxisX));
const float RawSlopeAngle = FMath::RadiansToDegrees(RawSlopeAngleRadians);
const float SlopeAngle = FMath::Fmod(FMath::UnwindDegrees(RawSlopeAngle) + 360.0f, 360.0f);
const FPaperTerrainMaterialRule* DesiredRule = (TerrainMaterial->Rules.Num() > 0) ? &(TerrainMaterial->Rules[0]) : nullptr;
for (const FPaperTerrainMaterialRule& TestRule : TerrainMaterial->Rules)
{
if ((SlopeAngle >= TestRule.MinimumAngle) && (SlopeAngle < TestRule.MaximumAngle))
{
DesiredRule = &TestRule;
}
}
if (ActiveSegment->Rule != DesiredRule)
{
if (ActiveSegment->Rule == nullptr)
{
ActiveSegment->Rule = DesiredRule;
}
else
{
ActiveSegment->EndTime = CurrentTime;
// Segment is too small, delete it
if (ActiveSegment->EndTime < ActiveSegment->StartTime + 2.0f * SegmentOverlapAmount)
{
Segments.Pop(false);
}
ActiveSegment = new (Segments)FTerrainSegment();
ActiveSegment->StartTime = CurrentTime;
ActiveSegment->EndTime = SplineLength;
ActiveSegment->Rule = DesiredRule;
}
}
CurrentTime += SlopeAnalysisTimeRate;
}
}
// Account for overlap
for (FTerrainSegment& Segment : Segments)
{
Segment.StartTime -= SegmentOverlapAmount;
Segment.EndTime += SegmentOverlapAmount;
}
// Convert those segments to actual geometry
for (FTerrainSegment& Segment : Segments)
{
check(Segment.Rule);
FTerrainRuleHelper RuleHelper(Segment.Rule);
float RemainingSegStart = Segment.StartTime + RuleHelper.StartWidth;
float RemainingSegEnd = Segment.EndTime - RuleHelper.EndWidth;
const float BodyDistance = RemainingSegEnd - RemainingSegStart;
float DistanceBudget = BodyDistance;
bool bUseBodySegments = (DistanceBudget > 0.0f) && (RuleHelper.ValidBodies.Num() > 0);
// Add the start cap
if (RuleHelper.StartWidth > 0.0f)
{
new (Segment.Stamps) FTerrainSpriteStamp(Segment.Rule->StartCap, Segment.StartTime + RuleHelper.StartWidth * 0.5f, /*bIsEndCap=*/ bUseBodySegments);
}
// Add body segments
if (bUseBodySegments)
{
int32 NumSegments = 0;
float Position = RemainingSegStart;
while (DistanceBudget > 0.0f)
{
const int32 BodyIndex = RuleHelper.GenerateBodyIndex(RandomStream);
const UPaperSprite* Sprite = RuleHelper.ValidBodies[BodyIndex];
const float Width = RuleHelper.ValidBodyWidths[BodyIndex];
if ((NumSegments > 0) && ((Width * 0.5f) > DistanceBudget))
{
break;
}
new (Segment.Stamps) FTerrainSpriteStamp(Sprite, Position + (Width * 0.5f), /*bIsEndCap=*/ false);
DistanceBudget -= Width;
Position += Width;
++NumSegments;
}
const float UsedSpace = (BodyDistance - DistanceBudget);
const float OverallScaleFactor = BodyDistance / UsedSpace;
// Stretch body segments
float PositionCorrectionSum = 0.0f;
for (int32 Index = 0; Index < NumSegments; ++Index)
{
FTerrainSpriteStamp& Stamp = Segment.Stamps[Index + (Segment.Stamps.Num() - NumSegments)];
const float WidthChange = (OverallScaleFactor - 1.0f) * Stamp.NominalWidth;
const float FirstGapIsSmallerFactor = (Index == 0) ? 0.5f : 1.0f;
PositionCorrectionSum += WidthChange * FirstGapIsSmallerFactor;
Stamp.Scale = OverallScaleFactor;
Stamp.Time += PositionCorrectionSum;
}
}
else
{
// Stretch endcaps
}
// Add the end cap
if (RuleHelper.EndWidth > 0.0f)
{
new (Segment.Stamps) FTerrainSpriteStamp(Segment.Rule->EndCap, Segment.EndTime - RuleHelper.EndWidth * 0.5f, /*bIsEndCap=*/ bUseBodySegments);
}
}
// Convert stamps into geometry
SpawnSegments(Segments, !bClosedSpline || (bClosedSpline && !bFilledSpline));
// Generate the background if the spline is closed
if (bClosedSpline && bFilledSpline)
{
// Create a polygon from the spline
FBox2D SplineBounds(ForceInit);
TArray<FVector2D> SplinePolyVertices2D;
TArray<float> SplineEdgeOffsetAmounts;
{
float CurrentTime = 0.0f;
while (CurrentTime < SplineLength)
{
const float Param = AssociatedSpline->SplineReparamTable.Eval(CurrentTime, 0.0f);
const FVector Position3D = AssociatedSpline->SplineInfo.Eval(Param, FVector::ZeroVector);
const FVector2D Position2D = FVector2D(FVector::DotProduct(Position3D, PaperAxisX), FVector::DotProduct(Position3D, PaperAxisY));
SplineBounds += Position2D;
SplinePolyVertices2D.Add(Position2D);
// Find the collision offset for this sample point
float CollisionOffset = 0;
for (int SegmentIndex = 0; SegmentIndex < Segments.Num(); ++SegmentIndex)
{
FTerrainSegment& Segment = Segments[SegmentIndex];
if (CurrentTime >= Segment.StartTime && CurrentTime <= Segment.EndTime)
{
CollisionOffset = (Segment.Rule != nullptr) ? (Segment.Rule->CollisionOffset * 0.25f) : 0;
break;
}
}
SplineEdgeOffsetAmounts.Add(CollisionOffset);
CurrentTime += FillRasterizationTimeRate;
}
}
SimplifyPolygon(SplinePolyVertices2D, SplineEdgeOffsetAmounts);
// Always CCW and facing forward regardless of spline winding
TArray<FVector2D> CorrectedSplineVertices;
PaperGeomTools::CorrectPolygonWinding(CorrectedSplineVertices, SplinePolyVertices2D, false);
TArray<FVector2D> TriangulatedPolygonVertices;
PaperGeomTools::TriangulatePoly(/*out*/TriangulatedPolygonVertices, CorrectedSplineVertices, false);
GenerateCollisionDataFromPolygon(SplinePolyVertices2D, SplineEdgeOffsetAmounts, TriangulatedPolygonVertices);
if (TerrainMaterial->InteriorFill != nullptr)
{
const UPaperSprite* FillSprite = TerrainMaterial->InteriorFill;
FPaperTerrainSpriteGeometry& MaterialBatch = *new (GeneratedSpriteGeometry)FPaperTerrainSpriteGeometry(); //@TODO: Look up the existing one instead
MaterialBatch.Material = FillSprite->GetDefaultMaterial();
FSpriteDrawCallRecord& FillDrawCall = *new (MaterialBatch.Records) FSpriteDrawCallRecord();
FillDrawCall.BuildFromSprite(FillSprite);
FillDrawCall.RenderVerts.Empty();
FillDrawCall.Color = TerrainColor;
FillDrawCall.Destination = PaperAxisZ * 0.1f;
const FVector2D TextureSize = GetSpriteRenderDataBounds2D(FillSprite->BakedRenderData).GetSize();
const FVector2D SplineSize = SplineBounds.GetSize();
GenerateFillRenderDataFromPolygon(FillSprite, FillDrawCall, TextureSize, TriangulatedPolygonVertices);
//@TODO: Add support for the fill sprite being smaller than the entire texture
#if NOT_WORKING
const float StartingDivisionPointX = FMath::CeilToFloat(SplineBounds.Min.X / TextureSize.X);
const float StartingDivisionPointY = FMath::CeilToFloat(SplineBounds.Min.Y / TextureSize.Y);
FPoly VerticalRemainder = SplineAsPolygon;
for (float Y = StartingDivisionPointY; VerticalRemainder.Vertices.Num() > 0; Y += TextureSize.Y)
{
FPoly Top;
FPoly Bottom;
const FVector SplitBaseOuter = (Y * PaperAxisY);
VerticalRemainder.SplitWithPlane(SplitBaseOuter, -PaperAxisY, &Top, &Bottom, 1);
VerticalRemainder = Bottom;
FPoly HorizontalRemainder = Top;
for (float X = StartingDivisionPointX; HorizontalRemainder.Vertices.Num() > 0; X += TextureSize.X)
{
FPoly Left;
FPoly Right;
const FVector SplitBaseInner = (X * PaperAxisX) + (Y * PaperAxisY);
HorizontalRemainder.SplitWithPlane(SplitBaseInner, -PaperAxisX, &Left, &Right, 1);
HorizontalRemainder = Right;
//BROKEN, function no longer exists (split into 2 parts)
SpawnFromPoly(Segments, SplineEdgeOffsetAmounts, FillSprite, FillDrawCall, TextureSize, Left);
}
}
#endif
}
}
// Draw debug frames at the start and end of the spline
#if PAPER_TERRAIN_DRAW_DEBUG
{
const float Time = 5.0f;
{
FTransform WorldTransform = GetTransformAtDistance(0.0f) * ComponentToWorld;
DrawDebugCoordinateSystem(GetWorld(), WorldTransform.GetLocation(), FRotator(WorldTransform.GetRotation()), 30.0f, true, Time, SDPG_Foreground);
}
{
FTransform WorldTransform = GetTransformAtDistance(SplineLength) * ComponentToWorld;
DrawDebugCoordinateSystem(GetWorld(), WorldTransform.GetLocation(), FRotator(WorldTransform.GetRotation()), 30.0f, true, Time, SDPG_Foreground);
}
}
#endif
}
RecreateRenderState_Concurrent();
}
void UKismetMathLibrary::MinimumAreaRectangle(class UObject* WorldContextObject, const TArray<FVector>& InVerts, const FVector& SampleSurfaceNormal, FVector& OutRectCenter, FRotator& OutRectRotation, float& OutSideLengthX, float& OutSideLengthY, bool bDebugDraw)
{
float MinArea = -1.f;
float CurrentArea = -1.f;
FVector SupportVectorA, SupportVectorB;
FVector RectSideA, RectSideB;
float MinDotResultA, MinDotResultB, MaxDotResultA, MaxDotResultB;
FVector TestEdge;
float TestEdgeDot = 0.f;
FVector PolyNormal(0.f, 0.f, 1.f);
TArray<int32> PolyVertIndices;
// Bail if we receive an empty InVerts array
if( InVerts.Num() == 0 )
{
return;
}
// Compute the approximate normal of the poly, using the direction of SampleSurfaceNormal for guidance
PolyNormal = (InVerts[InVerts.Num() / 3] - InVerts[0]) ^ (InVerts[InVerts.Num() * 2 / 3] - InVerts[InVerts.Num() / 3]);
if( (PolyNormal | SampleSurfaceNormal) < 0.f )
{
PolyNormal = -PolyNormal;
}
// Transform the sample points to 2D
FMatrix SurfaceNormalMatrix = FRotationMatrix::MakeFromZX(PolyNormal, FVector(1.f, 0.f, 0.f));
TArray<FVector> TransformedVerts;
OutRectCenter = FVector(0.f);
for( int32 Idx = 0; Idx < InVerts.Num(); ++Idx )
{
OutRectCenter += InVerts[Idx];
TransformedVerts.Add(SurfaceNormalMatrix.InverseTransformVector(InVerts[Idx]));
}
OutRectCenter /= InVerts.Num();
// Compute the convex hull of the sample points
ConvexHull2D::ComputeConvexHull(TransformedVerts, PolyVertIndices);
// Minimum area rectangle as computed by http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
for( int32 Idx = 1; Idx < PolyVertIndices.Num() - 1; ++Idx )
{
SupportVectorA = (TransformedVerts[PolyVertIndices[Idx]] - TransformedVerts[PolyVertIndices[Idx-1]]).SafeNormal();
SupportVectorA.Z = 0.f;
SupportVectorB.X = -SupportVectorA.Y;
SupportVectorB.Y = SupportVectorA.X;
SupportVectorB.Z = 0.f;
MinDotResultA = MinDotResultB = MaxDotResultA = MaxDotResultB = 0.f;
for (int TestVertIdx = 1; TestVertIdx < PolyVertIndices.Num(); ++TestVertIdx )
{
TestEdge = TransformedVerts[PolyVertIndices[TestVertIdx]] - TransformedVerts[PolyVertIndices[0]];
TestEdgeDot = SupportVectorA | TestEdge;
if( TestEdgeDot < MinDotResultA )
{
MinDotResultA = TestEdgeDot;
}
else if(TestEdgeDot > MaxDotResultA )
{
MaxDotResultA = TestEdgeDot;
}
TestEdgeDot = SupportVectorB | TestEdge;
if( TestEdgeDot < MinDotResultB )
{
MinDotResultB = TestEdgeDot;
}
else if( TestEdgeDot > MaxDotResultB )
{
MaxDotResultB = TestEdgeDot;
}
}
CurrentArea = (MaxDotResultA - MinDotResultA) * (MaxDotResultB - MinDotResultB);
if( MinArea < 0.f || CurrentArea < MinArea )
{
MinArea = CurrentArea;
RectSideA = SupportVectorA * (MaxDotResultA - MinDotResultA);
RectSideB = SupportVectorB * (MaxDotResultB - MinDotResultB);
}
}
RectSideA = SurfaceNormalMatrix.TransformVector(RectSideA);
RectSideB = SurfaceNormalMatrix.TransformVector(RectSideB);
OutRectRotation = FRotationMatrix::MakeFromZX(PolyNormal, RectSideA).Rotator();
OutSideLengthX = RectSideA.Size();
OutSideLengthY = RectSideB.Size();
if( bDebugDraw )
{
DrawDebugSphere(GEngine->GetWorldFromContextObject(WorldContextObject), OutRectCenter, 10.f, 12, FColor::Yellow, true);
DrawDebugCoordinateSystem(GEngine->GetWorldFromContextObject(WorldContextObject), OutRectCenter, SurfaceNormalMatrix.Rotator(), 100.f, true);
DrawDebugLine(GEngine->GetWorldFromContextObject(WorldContextObject), OutRectCenter - RectSideA * 0.5f + FVector(0,0,10.f), OutRectCenter + RectSideA * 0.5f + FVector(0,0,10.f), FColor::Green, true,-1, 0, 5.f);
DrawDebugLine(GEngine->GetWorldFromContextObject(WorldContextObject), OutRectCenter - RectSideB * 0.5f + FVector(0,0,10.f), OutRectCenter + RectSideB * 0.5f + FVector(0,0,10.f), FColor::Blue, true,-1, 0, 5.f);
}
}
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;
}
}
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_AimOffsetLookAt::UpdateFromLookAtTarget(FPoseContext& LocalPoseContext)
{
const FBoneContainer& RequiredBones = LocalPoseContext.Pose.GetBoneContainer();
if (RequiredBones.GetSkeletalMeshAsset())
{
const USkeletalMeshSocket* Socket = RequiredBones.GetSkeletalMeshAsset()->FindSocket(SourceSocketName);
if (Socket)
{
const FTransform SocketLocalTransform = Socket->GetSocketLocalTransform();
FBoneReference SocketBoneReference;
SocketBoneReference.BoneName = Socket->BoneName;
SocketBoneReference.Initialize(RequiredBones);
if (SocketBoneReference.IsValid(RequiredBones))
{
const FCompactPoseBoneIndex SocketBoneIndex = SocketBoneReference.GetCompactPoseIndex(RequiredBones);
FCSPose<FCompactPose> GlobalPose;
GlobalPose.InitPose(LocalPoseContext.Pose);
USkeletalMeshComponent* Component = LocalPoseContext.AnimInstanceProxy->GetSkelMeshComponent();
AActor* Actor = Component ? Component->GetOwner() : nullptr;
if (Component && Actor && BlendSpace)
{
const FTransform ActorTransform = Actor->GetTransform();
const FTransform BoneTransform = GlobalPose.GetComponentSpaceTransform(SocketBoneIndex);
const FTransform SocketWorldTransform = SocketLocalTransform * BoneTransform * Component->ComponentToWorld;
// Convert Target to Actor Space
const FTransform TargetWorldTransform(LookAtLocation);
const FVector DirectionToTarget = ActorTransform.InverseTransformVectorNoScale(TargetWorldTransform.GetLocation() - SocketWorldTransform.GetLocation()).GetSafeNormal();
const FVector CurrentDirection = ActorTransform.InverseTransformVectorNoScale(SocketWorldTransform.GetUnitAxis(EAxis::X));
const FVector AxisX = FVector::ForwardVector;
const FVector AxisY = FVector::RightVector;
const FVector AxisZ = FVector::UpVector;
const FVector2D CurrentCoords = FMath::GetAzimuthAndElevation(CurrentDirection, AxisX, AxisY, AxisZ);
const FVector2D TargetCoords = FMath::GetAzimuthAndElevation(DirectionToTarget, AxisX, AxisY, AxisZ);
const FVector BlendInput(
FRotator::NormalizeAxis(FMath::RadiansToDegrees(TargetCoords.X - CurrentCoords.X)),
FRotator::NormalizeAxis(FMath::RadiansToDegrees(TargetCoords.Y - CurrentCoords.Y)),
0.f);
// Set X and Y, so ticking next frame is based on correct weights.
X = BlendInput.X;
Y = BlendInput.Y;
// Generate BlendSampleDataCache from inputs.
BlendSpace->GetSamplesFromBlendInput(BlendInput, BlendSampleDataCache);
if (CVarAimOffsetLookAtDebug.GetValueOnAnyThread() == 1)
{
DrawDebugLine(Component->GetWorld(), SocketWorldTransform.GetLocation(), TargetWorldTransform.GetLocation(), FColor::Green);
DrawDebugLine(Component->GetWorld(), SocketWorldTransform.GetLocation(), SocketWorldTransform.GetLocation() + SocketWorldTransform.GetUnitAxis(EAxis::X) * (TargetWorldTransform.GetLocation() - SocketWorldTransform.GetLocation()).Size(), FColor::Red);
DrawDebugCoordinateSystem(Component->GetWorld(), ActorTransform.GetLocation(), ActorTransform.GetRotation().Rotator(), 100.f);
FString DebugString = FString::Printf(TEXT("Socket (X:%f, Y:%f), Target (X:%f, Y:%f), Result (X:%f, Y:%f)")
, FMath::RadiansToDegrees(CurrentCoords.X)
, FMath::RadiansToDegrees(CurrentCoords.Y)
, FMath::RadiansToDegrees(TargetCoords.X)
, FMath::RadiansToDegrees(TargetCoords.Y)
, BlendInput.X
, BlendInput.Y);
GEngine->AddOnScreenDebugMessage(INDEX_NONE, 0.f, FColor::Red, DebugString, false);
}
}
}
}
}
}
