void FConstraintInstance::SetRefFrame(EConstraintFrame::Type Frame, const FTransform& RefFrame)
{
#if WITH_PHYSX
	PxJointActorIndex::Enum PxFrame = U2PConstraintFrame(Frame);
#endif
	if(Frame == EConstraintFrame::Frame1)
	{
		Pos1 = RefFrame.GetTranslation();
		PriAxis1 = RefFrame.GetUnitAxis( EAxis::X );
		SecAxis1 = RefFrame.GetUnitAxis( EAxis::Y );
	}
	else
	{
		Pos2 = RefFrame.GetTranslation();
		PriAxis2 = RefFrame.GetUnitAxis( EAxis::X );
		SecAxis2 = RefFrame.GetUnitAxis( EAxis::Y );
	}

#if WITH_PHYSX
	if (PxD6Joint* Joint = GetUnbrokenJoint())
	{
		PxTransform PxRefFrame = U2PTransform(RefFrame);
		Joint->setLocalPose(PxFrame, PxRefFrame);
	}
#endif

}
FVector FAnimNode_LookAt::GetAlignVector(const FTransform& Transform, EAxisOption::Type AxisOption)
{
	switch(AxisOption)
	{
	case EAxisOption::X:
	case EAxisOption::X_Neg:
	return Transform.GetUnitAxis(EAxis::X);
	case EAxisOption::Y:
	case EAxisOption::Y_Neg:
	return Transform.GetUnitAxis(EAxis::Y);
	case EAxisOption::Z:
	case EAxisOption::Z_Neg:
	return Transform.GetUnitAxis(EAxis::Z);
	}

	return FVector(1.f, 0.f, 0.f);
}
float FAttenuationSettings::AttenuationEvalCone(const FTransform& SoundTransform, const FVector ListenerLocation, const float DistanceScale) const
{
    const FVector SoundForward = SoundTransform.GetUnitAxis( EAxis::X );

    float VolumeMultiplier = 1.f;

    const FVector Origin = SoundTransform.GetTranslation() - (SoundForward * ConeOffset);

    const float Distance = FMath::Max(FVector::Dist( Origin, ListenerLocation ) - AttenuationShapeExtents.X, 0.f);
    VolumeMultiplier *= AttenuationEval(Distance, FalloffDistance, DistanceScale);

    if (VolumeMultiplier > 0.f)
    {
        const float theta = FMath::RadiansToDegrees(fabsf(FMath::Acos( FVector::DotProduct(SoundForward, (ListenerLocation - Origin).GetSafeNormal()))));
        VolumeMultiplier *= AttenuationEval(theta - AttenuationShapeExtents.Y, AttenuationShapeExtents.Z, 1.0f);
    }

    return VolumeMultiplier;
}
float FAttenuationSettings::AttenuationEvalCapsule(const FTransform& SoundTransform, const FVector ListenerLocation, const float DistanceScale) const
{
    float Distance = 0.f;
    const float CapsuleHalfHeight = AttenuationShapeExtents.X;
    const float CapsuleRadius = AttenuationShapeExtents.Y;

    // Capsule devolves to a sphere if HalfHeight <= Radius
    if (CapsuleHalfHeight <= CapsuleRadius )
    {
        Distance = FMath::Max(FVector::Dist( SoundTransform.GetTranslation(), ListenerLocation ) - CapsuleRadius, 0.f);
    }
    else
    {
        const FVector PointOffset = (CapsuleHalfHeight - CapsuleRadius) * SoundTransform.GetUnitAxis( EAxis::Z );
        const FVector StartPoint = SoundTransform.GetTranslation() + PointOffset;
        const FVector EndPoint = SoundTransform.GetTranslation() - PointOffset;

        Distance = FMath::PointDistToSegment(ListenerLocation, StartPoint, EndPoint) - CapsuleRadius;
    }

    return AttenuationEval(Distance, FalloffDistance, DistanceScale);
}
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);
                    }
                }
            }
        }
    }
}