Plugin/Source/SenseGlove/Private/SenseGlove/Components/SGVirtualHandComponent.cpp

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2023 SenseGlove
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * @section DESCRIPTION
 *
 *
 */


#include "SenseGlove/Components/SGVirtualHandComponent.h"

#include "Animation/Skeleton.h"
#include "Components/SkeletalMeshComponent.h"
#include "Engine/SkeletalMesh.h"
#include "UObject/ConstructorHelpers.h"

#include "SenseGlove/Animation/SGVirtualHandAnimInstance.h"
#include "SGCore/SGHandPose.h"
#include "SGCore/SGHapticGlove.h"
#include "SGLog/SGLog.h"

struct USGVirtualHandComponent::FImpl
{
    /************************
     * Static methods
     ************************/

    FORCEINLINE static const FString& GetDefaultLeftHandMeshPath()
    {
        static const FString Path{TEXT("SkeletalMesh'/SenseGlove/Meshes/"
            "SK_SenseGlove_VirtualHand_Left.SK_SenseGlove_VirtualHand_Left'")};
        return Path;
    }

    FORCEINLINE static const FString& GetDefaultLeftHandMeshPathOnly()
    {
        static const FString Path{TEXT("/SenseGlove/Meshes/"
            "SK_SenseGlove_VirtualHand_Left.SK_SenseGlove_VirtualHand_Left")};
        return Path;
    }

    FORCEINLINE static const FString& GetDefaultRightHandMeshPath()
    {
        static const FString Path{TEXT("SkeletalMesh'/SenseGlove/Meshes/"
            "SK_SenseGlove_VirtualHand_Right.SK_SenseGlove_VirtualHand_Right'")};
        return Path;
    }

    FORCEINLINE static const FString& GetDefaultRightHandMeshPathOnly()
    {
        static const FString Path{TEXT("/SenseGlove/Meshes/"
            "SK_SenseGlove_VirtualHand_Right.SK_SenseGlove_VirtualHand_Right")};
        return Path;
    }

    /************************
     * Owner object
     ************************/

    USGVirtualHandComponent* Owner;

    /************************
     * Constructor / Destructor
     ************************/

    explicit FImpl(USGVirtualHandComponent* InOwner);
    ~FImpl();

    /************************
     * Default copy constructor & copy assignment operator
     ************************/

    FImpl(const FImpl& Rhs) = default;
    FImpl& operator=(const FImpl& Rhs) = default;

    /************************
     * Methods
     ************************/

    bool IsEditor() const;

    FString GetDefaultMeshPath() const;
    bool IsUsingUserMesh() const;
    USkeletalMesh* LoadDefaultMesh() const;
    void SetDefaultMesh() const;

    USkeletalMesh* GetMesh() const;
    USkeleton* GetSkeleton() const;
    const FReferenceSkeleton& GetRefSkeleton() const;

    bool CheckGlove() const;
};

const TArray<TArray<FName>>& USGVirtualHandComponent::GetLeftHandFingerBoneNames()
{
    static const TArray<TArray<FName>> BoneNames{
        {FName(TEXT("thumb_01_l")), FName(TEXT("thumb_02_l")), FName(TEXT("thumb_03_l"))},
        {FName(TEXT("index_01_l")), FName(TEXT("index_02_l")), FName(TEXT("index_03_l"))},
        {FName(TEXT("middle_01_l")), FName(TEXT("middle_02_l")), FName(TEXT("middle_03_l"))},
        {FName(TEXT("ring_01_l")), FName(TEXT("ring_02_l")), FName(TEXT("ring_03_l"))},
        {FName(TEXT("pinky_01_l")), FName(TEXT("pinky_02_l")), FName(TEXT("pinky_03_l"))},
    };
    return BoneNames;
}

const TArray<TArray<FName>>& USGVirtualHandComponent::GetRightHandFingerBoneNames()
{
    static const TArray<TArray<FName>> BoneNames{
        {FName(TEXT("thumb_01_r")), FName(TEXT("thumb_02_r")), FName(TEXT("thumb_03_r"))},
        {FName(TEXT("index_01_r")), FName(TEXT("index_02_r")), FName(TEXT("index_03_r"))},
        {FName(TEXT("middle_01_r")), FName(TEXT("middle_02_r")), FName(TEXT("middle_03_r"))},
        {FName(TEXT("ring_01_r")), FName(TEXT("ring_02_r")), FName(TEXT("ring_03_r"))},
        {FName(TEXT("pinky_01_r")), FName(TEXT("pinky_02_r")), FName(TEXT("pinky_03_r"))},
    };
    return BoneNames;
}

const FName& USGVirtualHandComponent::GetLeftHandFingerBoneName(const int32 Finger, const int32 Joint)
{
    const TArray<TArray<FName>>& BoneNames{GetLeftHandFingerBoneNames()};

    ensureAlwaysMsgf(Finger < BoneNames.Num(), TEXT("Finger index is out of bound!"));
    ensureAlwaysMsgf(Joint < BoneNames[Finger].Num(), TEXT("Joint index is out of bound!"));

    const FName& BoneName{BoneNames[Finger][Joint]};
    return BoneName;
}

const FName& USGVirtualHandComponent::GetRightHandFingerBoneName(const int32 Finger, const int32 Joint)
{
    const TArray<TArray<FName>>& BoneNames{GetRightHandFingerBoneNames()};

    ensureAlwaysMsgf(Finger < BoneNames.Num(), TEXT("Finger index is out of bound!"));
    ensureAlwaysMsgf(Joint < BoneNames[Finger].Num(), TEXT("Joint index is out of bound!"));

    const FName& BoneName{BoneNames[Finger][Joint]};
    return BoneName;
}

USGVirtualHandComponent::USGVirtualHandComponent(const FObjectInitializer& ObjectInitializer)
    : Super(ObjectInitializer),
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    PrimaryComponentTick.bTickEvenWhenPaused = false;
    PrimaryComponentTick.bCanEverTick = true;
    PrimaryComponentTick.bStartWithTickEnabled = true;
    bTickInEditor = true;
    bNeverNeedsRenderUpdate = false;
    bAllowConcurrentTick = true;
    SetUpdateAnimationInEditor(true);

    SetCastShadow(true);
    SetCastHiddenShadow(false);
    bCastDynamicShadow = true;
    SetOnlyOwnerSee(false);
    SetReceivesDecals(true);
    SetCanEverAffectNavigation(false);
    Super::SetCollisionObjectType(ECC_WorldDynamic);
    Super::SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
    Super::SetCollisionResponseToAllChannels(ECR_Overlap);
    Super::SetCollisionResponseToChannel(ECC_WorldStatic, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_WorldDynamic, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_Pawn, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_Visibility, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_PhysicsBody, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_Vehicle, ECR_Block);
    Super::SetCollisionResponseToChannel(ECC_Destructible, ECR_Block);

    Super::SetAnimClass(USGVirtualHandAnimInstance::StaticClass());

    bRight = true;
    bHiddenInGameIfNoGloveDetected = true;

    const FString SkeletalMeshPath(Pimpl->GetDefaultMeshPath());
    const ConstructorHelpers::FObjectFinder<USkeletalMesh> SkeletalMesh(*SkeletalMeshPath);
    ensureAlwaysMsgf(SkeletalMesh.Succeeded() && SkeletalMesh.Object,
                     TEXT("Failed to load the skeletal mesh: '%s'"), *SkeletalMeshPath);
    Super::SetSkeletalMesh(SkeletalMesh.Object, true);

    Glove = nullptr;
}

void USGVirtualHandComponent::SetRight(const bool bInRight)
{
    bRight = bInRight;

    if (!Pimpl->IsUsingUserMesh())
    {
        Pimpl->SetDefaultMesh();
    }
}

void USGVirtualHandComponent::SetAnimClass(UClass* NewClass)
{
    ensureAlwaysMsgf(Cast<USGVirtualHandAnimInstance>(NewClass),
                     TEXT("The AnimInstance class must be a subclass of USGVirtualHandAnimInstance!"));

    Super::SetAnimClass(NewClass);
}

#if WITH_EDITOR
void USGVirtualHandComponent::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent)
{
    Super::PostEditChangeProperty(PropertyChangedEvent);

    const FName PropertyName{PropertyChangedEvent.Property
        ? PropertyChangedEvent.Property->GetFName()
        : NAME_None};
    if (PropertyName == GET_MEMBER_NAME_CHECKED(USGVirtualHandComponent, bRight))
    {
        if (!Pimpl->IsUsingUserMesh())
        {
            Pimpl->SetDefaultMesh();
        }
    }
}
#endif  /* WITH_EDITOR */

void USGVirtualHandComponent::InitializeComponent()
{
    Super::InitializeComponent();

    if (!Pimpl->IsUsingUserMesh())
    {
        Pimpl->SetDefaultMesh();
    }
}

void USGVirtualHandComponent::TickComponent(
    const float DeltaTime, const ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
#if WITH_EDITOR
    if (Pimpl->IsEditor())
    {
        EditorTick(DeltaTime, TickType, ThisTickFunction);
        return;
    }
#endif

    Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

    (void) Pimpl->CheckGlove();
}

void USGVirtualHandComponent::SetSkeletalMesh(USkeletalMesh* NewMesh, const bool bReinitPose)
{
    if (IsValid(NewMesh))
    {
        Super::SetSkeletalMesh(NewMesh, bReinitPose);
    }
    else
    {
        Super::SetSkeletalMesh(Pimpl->LoadDefaultMesh(), bReinitPose);
    }
}

void USGVirtualHandComponent::EditorTick(
    const float DeltaTime, const ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
    (void) Pimpl->CheckGlove();
}


bool USGVirtualHandComponent::IsGloveConnected() const
{
    return Pimpl->CheckGlove();
}

USGHandPose* USGVirtualHandComponent::GetHandPose()
{
    USGHandPose* HandPose = nullptr;

    if (IsValid(Glove))
    {
        Glove->GetHandPose(this, HandPose);
    }

    return HandPose;
}

const FName& USGVirtualHandComponent::GetFingerBoneName(const int32 Finger, const int32 Joint) const
{
    const FName& Name{IsRight()
        ? GetRightHandFingerBoneName(Finger, Joint)
        : GetLeftHandFingerBoneName(Finger, Joint)};
    return Name;
}

const FTransform& USGVirtualHandComponent::GetFingerBoneRefTransform(const int32 Finger, const int32 Joint) const
{
    const FName& BoneName{GetFingerBoneName(Finger, Joint)};
    const FReferenceSkeleton& ReferenceSkeleton{Pimpl->GetRefSkeleton()};
    const int32 BoneIndex = ReferenceSkeleton.FindBoneIndex(BoneName);

    ensureAlwaysMsgf(BoneIndex != INDEX_NONE, TEXT("Invalid virtual hand bone name '%s' or index!"),
                     *BoneName.ToString());

    const FTransform& BoneTransform{ReferenceSkeleton.GetRefBonePose()[BoneIndex]};
    return BoneTransform;
}

FRotator USGVirtualHandComponent::GetFingerBoneRefRotation(const int32 Finger, const int32 Joint) const
{
    const FTransform BoneTransform{GetFingerBoneRefTransform(Finger, Joint)};
    FRotator BoneRotation{BoneTransform.GetRotation().Rotator()};
    return MoveTemp(BoneRotation);
}

TMap<FName, FRotator> USGVirtualHandComponent::GetFingersBonesRotations()
{
    TMap<FName, FRotator> Rotations;

    if(!IsGloveConnected())
    {
        return MoveTemp(Rotations);
    }

    const USGHandPose* HandPose = GetHandPose();
    if (!IsValid(HandPose))
    {
        return MoveTemp(Rotations);
    }

    const TArray<TArray<FVector>> HandAngles{HandPose->GetHandAngles()};

    for (int32 FingerIndex = 0; FingerIndex < HandAngles.Num(); ++FingerIndex)
    {
        const TArray<FVector>& FingerJointAngles{HandAngles[FingerIndex]};

        for (int32 JointIndex = 0; JointIndex < FingerJointAngles.Num(); ++JointIndex)
        {
            const FVector& JointAngle{FingerJointAngles[JointIndex]};
            const FQuat JointRotation{FQuat::MakeFromEuler(JointAngle)};

            const FTransform& BoneRefTransform{GetFingerBoneRefTransform(FingerIndex, JointIndex)};

            FQuat NewBoneRotation{BoneRefTransform.GetRotation() * JointRotation};

            FName BoneName{GetFingerBoneName(FingerIndex, JointIndex)};

            Rotations.Emplace(MoveTemp(BoneName), MoveTemp(NewBoneRotation));
        }
    }

    return MoveTemp(Rotations);
}

USGVirtualHandComponent::FImpl::FImpl(USGVirtualHandComponent* InOwner)
    : Owner(InOwner)
{
}

USGVirtualHandComponent::FImpl::~FImpl() = default;

bool USGVirtualHandComponent::FImpl::IsEditor() const
{
    const UWorld* World = Owner->GetWorld();
    if (World && World->WorldType == EWorldType::Editor)
    {
        return true;
    }

    return false;
}

FString USGVirtualHandComponent::FImpl::GetDefaultMeshPath() const
{
    FString SkeletalMeshPath;

    if (Owner->bRight)
    {
        SkeletalMeshPath = GetDefaultRightHandMeshPath();
    }
    else
    {
        SkeletalMeshPath = GetDefaultLeftHandMeshPath();
    }

    return MoveTemp(SkeletalMeshPath);
}

bool USGVirtualHandComponent::FImpl::IsUsingUserMesh() const
{
    const USkeletalMesh* SkeletalMesh{GetMesh()};
    if (!IsValid(SkeletalMesh))
    {
        return false;
    }

    const FString CurrentMeshPath(SkeletalMesh->GetPathName());
    const FString LeftHandDefaultMeshPath(GetDefaultLeftHandMeshPath());
    const FString LeftHandDefaultMeshPathOnly(GetDefaultLeftHandMeshPathOnly());
    const FString RightHandDefaultMeshPath(GetDefaultRightHandMeshPath());
    const FString RightHandDefaultMeshPathOnly(GetDefaultRightHandMeshPathOnly());
    if (CurrentMeshPath == LeftHandDefaultMeshPath
        || CurrentMeshPath == LeftHandDefaultMeshPathOnly
        || CurrentMeshPath == RightHandDefaultMeshPath
        || CurrentMeshPath == RightHandDefaultMeshPathOnly)
    {
        return false;
    }

    return true;
}

USkeletalMesh* USGVirtualHandComponent::FImpl::LoadDefaultMesh() const
{
    const FString SkeletalMeshPath(GetDefaultMeshPath());
    USkeletalMesh* SkeletalMesh = Cast<USkeletalMesh>(
        StaticLoadObject(USkeletalMesh::StaticClass(), nullptr, *SkeletalMeshPath));

    ensureAlwaysMsgf(SkeletalMesh, TEXT("Failed to load the skeletal mesh: '%s'"), *SkeletalMeshPath);

    return SkeletalMesh;
}

void USGVirtualHandComponent::FImpl::SetDefaultMesh() const
{
    Owner->SetSkeletalMesh(LoadDefaultMesh(), true);
}

USkeletalMesh* USGVirtualHandComponent::FImpl::GetMesh() const
{
    return Owner->
#if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 1
        GetSkeletalMeshAsset()
#else
        SkeletalMesh
#endif  /* ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION >= 1 */
        ;
}

USkeleton* USGVirtualHandComponent::FImpl::GetSkeleton() const
{
    USkeletalMesh* SkeletalMesh{GetMesh()};
    ensureAlwaysMsgf(IsValid(SkeletalMesh), TEXT("Invalid SkeletalMesh object!"));

    return SkeletalMesh->
#if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 1
        GetSkeleton()
#else
        Skeleton
#endif  /* ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION >= 1 */
        ;
}

const FReferenceSkeleton& USGVirtualHandComponent::FImpl::GetRefSkeleton() const
{
    USkeletalMesh* SkeletalMesh{GetMesh()};
    ensureAlwaysMsgf(IsValid(SkeletalMesh), TEXT("Invalid SkeletalMesh object!"));

    return SkeletalMesh->
#if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 1
        GetRefSkeleton()
#else
        RefSkeleton
#endif  /* ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION >= 1 */
        ;
}

bool USGVirtualHandComponent::FImpl::CheckGlove() const
{
    const bool bGloveWasPresent = IsValid(Owner->Glove);
    const bool bFoundGlove = USGHapticGlove::GetGlove(Owner, Owner->IsRight(), Owner->Glove);

    if (!bFoundGlove && bGloveWasPresent)
    {
        SGLOG_ERROR(FString::Printf(TEXT("Could not find a %s glove"),
            (Owner->bRight ? TEXT("right-handed") : TEXT("left-handed"))));
    }

    return bFoundGlove;
}

void USGVirtualHandComponent::FImplDeleter::operator()(const FImpl* P) const
{
    delete P;
}