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

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2024 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/SGWristTrackerComponent.h"

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

#include "SenseGlove/SGHeadMountedDisplay.h"
#include "SGBackend/SGBackend.h"
#include "SGCore/SGHapticGlove.h"
#include "SGDebug/SGDebugGizmo.h"
#include "SGLog/SGLog.h"
#include "SGSettings/SGSettings.h"

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

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

    USGWristTrackerComponent* Owner;

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

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

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

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

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

    bool IsEditor() const;

    bool CheckGlove() const;

    void UpdateMotionSource() const;
    void UpdateWristTrackingData() const;
    void DrawDebugGizmo();
};

USGWristTrackerComponent::USGWristTrackerComponent(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 = true;
    bAllowConcurrentTick = true;

    SetCastShadow(false);
    SetCastHiddenShadow(false);
    bCastDynamicShadow = false;
    SetOnlyOwnerSee(false);
    SetReceivesDecals(false);
    SetCanEverAffectNavigation(false);

    bOverridePluginWristTrackingSettings = false;
    WristTrackingSettings = FSGWristTrackingSettings(
        ESGPositionalTrackingHardware::None,
        FVector::ZeroVector,
        FVector::ZeroVector,
        FRotator::ZeroRotator,
        FRotator::ZeroRotator,
        EControllerHand::Left,
        EControllerHand::Right,
        false,
        FSGDebugGizmoSettings(
            4.0f, 1.0f,
            FColor(255, 0, 0, 255),
            FColor(0, 255, 0, 255),
            FColor(0, 0, 255, 255),
            false, 1.1f, 0
            )
        );

    bRight = true;

    Glove = nullptr;
    WristLocation = FVector::ZeroVector;
    WristRotation = FRotator::ZeroRotator;
}

void USGWristTrackerComponent::SetRight(const bool bInRight)
{
    bRight = bInRight;
    Pimpl->UpdateMotionSource();
}

const FSGWristTrackingSettings& USGWristTrackerComponent::GetWristTrackingSettings() const
{
    if (OverridesPluginWristTrackingSettings())
    {
        return WristTrackingSettings;
    }

    const USGSettings* Settings{USGSettings::GetInstance()};
    if (!ensureAlwaysMsgf(Settings, TEXT("The settings subsystem has not been initialized!")))
    {
        return WristTrackingSettings;
    }

    return Settings->GetWristTrackingSettings();
}

void USGWristTrackerComponent::SetWristTrackingSettings(const FSGWristTrackingSettings& InWristTrackingSettings)
{
    ensureAlwaysMsgf(
        OverridesPluginWristTrackingSettings(),
        TEXT( "Setting WristTrackingSettings is only permitted when bOverridePluginWristTrackingSettings is enabled!"));

    WristTrackingSettings = InWristTrackingSettings;
}

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

    const FName PropertyName{
        PropertyChangedEvent.Property
        ? PropertyChangedEvent.Property->GetFName()
        : NAME_None
    };

    (void) PropertyName;
}
#endif  /* WITH_EDITOR */

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

    (void) Pimpl->CheckGlove();

    Pimpl->UpdateMotionSource();
}

void USGWristTrackerComponent::BeginPlay()
{
    Super::BeginPlay();

    (void) Pimpl->CheckGlove();

    Pimpl->UpdateMotionSource();
}

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

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

    if (!!WristTrackingSettings.bDrawWristTrackerDebugGizmo)
    {
        Pimpl->DrawDebugGizmo();
    }

    (void) Pimpl->CheckGlove();
}


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

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

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

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

    return false;
}

bool USGWristTrackerComponent::FImpl::CheckGlove() const
{
    if (!USGBackend::GetInstance()->IsBackendInitialized())
    {
        const bool bSucceeded = USGBackend::GetInstance()->InitializeBackend();
        if (!bSucceeded)
        {
            return false;
        }
    }

    bool bGloveConnected;
    if (!IsValid(Owner->Glove) || !Owner->Glove->IsConnected())
    {
        bGloveConnected = USGHapticGlove::GetGlove(Owner, Owner->IsRight(), Owner->Glove);
        if (!bGloveConnected)
        {
            Owner->Glove = nullptr;
        }
    }
    else
    {
        bGloveConnected = true;
        UpdateWristTrackingData();
    }

    return bGloveConnected;
}

void USGWristTrackerComponent::FImpl::UpdateMotionSource() const
{
    if (GEngine)
    {
        Owner->SetTrackingSource(Owner->IsRight()
            ? Owner->GetWristTrackingSettings().RightHandMotionSource
            : Owner->GetWristTrackingSettings().LeftHandMotionSource);
    }
}

void USGWristTrackerComponent::FImpl::UpdateWristTrackingData() const
{
    ensureAlwaysMsgf(Owner->Glove, TEXT("Invalid Glove!"));

    FVector TrackerLocation{Owner->GetComponentLocation()};
    FRotator TrackerRotation{Owner->GetComponentRotation()};

    ESGPositionalTrackingHardware TrackerHardware = Owner->GetWristTrackingSettings().TrackingHardware;
    if (TrackerHardware == ESGPositionalTrackingHardware::None)
    {
        const ESGHeadMountedDisplayDevice HmdDevice = FSGHeadMountedDisplay::GetDeviceType();
        switch (HmdDevice)
        {
        case ESGHeadMountedDisplayDevice::MetaQuest2:
            TrackerHardware = ESGPositionalTrackingHardware::Quest2Controller;
            break;
        case ESGHeadMountedDisplayDevice::MetaQuestPro:
            TrackerHardware = ESGPositionalTrackingHardware::QuestProController;
            break;
        case ESGHeadMountedDisplayDevice::MetaQuest3:
            TrackerHardware = ESGPositionalTrackingHardware::Quest3Controller;
            break;
        case ESGHeadMountedDisplayDevice::HtcVivePro:
            TrackerHardware = ESGPositionalTrackingHardware::ViveTracker;
            break;
        case ESGHeadMountedDisplayDevice::HtcViveFocus3:
        // Fall through
        case ESGHeadMountedDisplayDevice::HtcViveXRElite:
            TrackerHardware = ESGPositionalTrackingHardware::ViveFocus3WristTracker;
            break;
        default:
            TrackerHardware = ESGPositionalTrackingHardware::Custom;
            break;
        }
    }
    else if (TrackerHardware == ESGPositionalTrackingHardware::Custom)
    {
        if (Owner->IsRight())
        {
            TrackerLocation += Owner->GetWristTrackingSettings().TrackingHardwareLocationOffsetRightHand;
            TrackerRotation += Owner->GetWristTrackingSettings().TrackingHardwareRotationOffsetRightHand;
        }
        else
        {
            TrackerLocation += Owner->GetWristTrackingSettings().TrackingHardwareLocationOffsetLeftHand;
            TrackerRotation += Owner->GetWristTrackingSettings().TrackingHardwareRotationOffsetLeftHand;
        }
    }

    FVector NewWristLocation;
    FRotator NewWristRotation;
    Owner->Glove->GetWristLocation(TrackerLocation, TrackerRotation, TrackerHardware,
                                   NewWristLocation, NewWristRotation);

    const bool bLocationUpdated = !Owner->WristLocation.Equals(NewWristLocation, SMALL_NUMBER);
    const bool bRotationUpdated = !Owner->WristRotation.Equals(NewWristRotation, SMALL_NUMBER);

    if (bLocationUpdated)
    {
        Owner->WristLocation = MoveTemp(NewWristLocation);
    }

    if (bRotationUpdated)
    {
        Owner->WristRotation = MoveTemp(NewWristRotation);
    }

    if (bLocationUpdated && Owner->WristLocationChangedEvent.IsBound())
    {
        Owner->WristLocationChangedEvent.Broadcast(Owner->WristLocation);
    }

    if (bRotationUpdated && Owner->WristRotationChangedEvent.IsBound())
    {
        Owner->WristRotationChangedEvent.Broadcast(Owner->WristRotation);
    }

    if ((bLocationUpdated && bRotationUpdated) && Owner->WristLocationAndRotationChangedEvent.IsBound())
    {
        Owner->WristLocationAndRotationChangedEvent.Broadcast(Owner->WristLocation, Owner->WristRotation);
    }
}

void USGWristTrackerComponent::FImpl::DrawDebugGizmo()
{
    const UWorld* World{Owner->GetWorld()};
    const FVector& Location{Owner->GetComponentLocation()};
    const FRotator& Rotation{Owner->GetComponentRotation()};
    const FSGDebugGizmoSettings& DebugGizmoSettings{Owner->GetWristTrackingSettings().WristTrackerDebugGizmoSettings};

    FSGDebugGizmo::Draw(World, Location, Rotation, DebugGizmoSettings);
}

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