Plugin/Source/SenseGlove/Private/SenseGlove/GameFramework/SGVirtualHand.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/GameFramework/SGVirtualHand.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 ASGVirtualHand::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
     ************************/

    ASGVirtualHand* Owner;

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

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

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

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

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

    bool IsEditor() const;

    void SetHandMesh(USkeletalMesh* VirtualHandMesh, bool bReinitPos) const;

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

    bool CheckGlove() const;
};

const TArray<TArray<FName>>& ASGVirtualHand::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>>& ASGVirtualHand::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& ASGVirtualHand::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& ASGVirtualHand::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;
}

ASGVirtualHand::ASGVirtualHand(const FObjectInitializer& ObjectInitializer)
    : Super(ObjectInitializer),
#if ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 24 )
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
#else
      Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}))
#endif  /* ENGINE_MAJOR_VERSION == 5 || ( ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION > 24 ) */
{
    PrimaryActorTick.bCanEverTick = true;

    SceneRoot = CreateDefaultSubobject<USceneComponent>(TEXT("SceneRoot"));
    SetRootComponent(SceneRoot);

    Mesh = CreateDefaultSubobject<USkeletalMeshComponent>(TEXT("Mesh"));
    Mesh->SetupAttachment(RootComponent);
    Mesh->SetCastShadow(true);
    Mesh->SetCastHiddenShadow(false);
    Mesh->bCastDynamicShadow = true;
    Mesh->SetOnlyOwnerSee(false);
    Mesh->SetReceivesDecals(true);
    Mesh->SetCanEverAffectNavigation(false);
    Mesh->SetCollisionObjectType(ECC_WorldDynamic);
    Mesh->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
    Mesh->SetCollisionResponseToAllChannels(ECR_Overlap);
    Mesh->SetCollisionResponseToChannel(ECC_WorldStatic, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_WorldDynamic, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_Pawn, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_Visibility, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_PhysicsBody, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_Vehicle, ECR_Block);
    Mesh->SetCollisionResponseToChannel(ECC_Destructible, ECR_Block);

    AnimInstanceClass = USGVirtualHandAnimInstance::StaticClass();

    bRight = true;
    bHiddenInGameIfNoGloveDetected = true;
    bAnimateInEditor = 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);
    Pimpl->SetHandMesh(SkeletalMesh.Object, true);

    Glove = nullptr;
}

void ASGVirtualHand::Tick(const float DeltaSeconds)
{
#if WITH_EDITOR
    if (Pimpl->IsEditor())
    {
        EditorTick(DeltaSeconds);
        return;
    }
#endif

    Super::Tick(DeltaSeconds);

    (void) Pimpl->CheckGlove();
}

bool ASGVirtualHand::ShouldTickIfViewportsOnly() const
{
    if (!bAnimateInEditor)
    {
        return false;
    }

    if (Pimpl->IsEditor())
    {
        return true;
    }

    return false;
}

void ASGVirtualHand::OnConstruction(const FTransform& Transform)
{
    Super::OnConstruction(Transform);

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

void ASGVirtualHand::EditorTick(float DeltaSeconds)
{
    (void) Pimpl->CheckGlove();
}

USkeletalMesh* ASGVirtualHand::GetHandMesh() const
{
    ensureAlwaysMsgf(IsValid(Mesh), TEXT("Invalid Mesh object!"));

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

void ASGVirtualHand::SetHandMesh(USkeletalMesh* VirtualHandMesh, const bool bReinitPos) const
{
    Pimpl->SetHandMesh(VirtualHandMesh, bReinitPos);
}

USkeleton* ASGVirtualHand::GetHandSkeleton() const
{
    USkeletalMesh* SkeletalMesh{GetHandMesh()};
    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& ASGVirtualHand::GetHandRefSkeleton() const
{
    USkeletalMesh* SkeletalMesh{GetHandMesh()};
    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 ASGVirtualHand::IsGloveConnected() const
{
    return Pimpl->CheckGlove();
}

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

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

    return HandPose;
}

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

const FTransform& ASGVirtualHand::GetFingerBoneRefTransform(const int32 Finger, const int32 Joint) const
{
    const FName& BoneName{GetFingerBoneName(Finger, Joint)};
    const FReferenceSkeleton& ReferenceSkeleton{GetHandRefSkeleton()};
    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 ASGVirtualHand::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> ASGVirtualHand::GetFingersBonesRotations()
{
    TMap<FName, FRotator> 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);
}

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

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

void ASGVirtualHand::FImpl::SetHandMesh(USkeletalMesh* VirtualHandMesh, const bool bReinitPos) const
{
    ensureAlwaysMsgf(IsValid(VirtualHandMesh), TEXT("Invalid VirtualHandMesh object!"));
    ensureAlwaysMsgf(IsValid(Owner->Mesh), TEXT("Invalid Mesh object!"));
    ensureAlwaysMsgf(IsValid(Owner->AnimInstanceClass), TEXT("Invalid AnimInstanceClass object!"));

    Owner->Mesh->SetSkeletalMesh(VirtualHandMesh, bReinitPos);
    Owner->Mesh->SetAnimInstanceClass(Owner->AnimInstanceClass);
}

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

    return false;
}

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

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

    return MoveTemp(SkeletalMeshPath);
}

bool ASGVirtualHand::FImpl::IsUsingUserMesh() const
{
    const USkeletalMesh* SkeletalMesh{Owner->GetHandMesh()};
    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* ASGVirtualHand::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 ASGVirtualHand::FImpl::SetDefaultMesh() const
{
    SetHandMesh(LoadDefaultMesh(), true);
}

bool ASGVirtualHand::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 ASGVirtualHand::FImplDeleter::operator()(const FImpl* P) const
{
    delete P;
}