fix a few wrist tracker issues and introduce SGWristTrackerVisualizationComponent

2024-08-16 18:00:54 +02:00
parent 01f487a87c
commit 6fc9c91b4a
10 changed files with 484 additions and 104 deletions
@@ -139,6 +139,136 @@ struct FSGXRTracker::FImpl
        return Settings->GetTrackingSettings().WristTrackingSettings;
    }

+    FORCEINLINE static const FName& GetDefaultLeftMotionSourceName()
+    {
+        static const FName Name(TEXT("Left"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetDefaultRightMotionSourceName()
+    {
+        static const FName Name(TEXT("Right"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetDefaultMotionSourceName(const EControllerHand Hand)
+    {
+        const FName& Name{
+            Hand == EControllerHand::Right
+            ? GetDefaultRightMotionSourceName()
+            : GetDefaultLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetDefaultMotionSourceName(const bool bRight)
+    {
+        const FName& Name{
+            bRight
+            ? GetDefaultRightMotionSourceName()
+            : GetDefaultLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetLegacyControllerLeftMotionSourceName()
+    {
+        static const FName Name(TEXT("Left"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetLegacyControllerRightMotionSourceName()
+    {
+        static const FName Name(TEXT("Right"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetLegacyControllerMotionSourceName(const EControllerHand Hand)
+    {
+        const FName& Name{
+            Hand == EControllerHand::Right
+            ? GetLegacyControllerRightMotionSourceName()
+            : GetLegacyControllerLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetLegacyControllerMotionSourceName(const bool bRight)
+    {
+        const FName& Name{
+            bRight
+            ? GetLegacyControllerRightMotionSourceName()
+            : GetLegacyControllerLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMoreSpecificLeftMotionSourceName()
+    {
+        static const FName Name(TEXT("HandTrackingLeft"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMoreSpecificRightMotionSourceName()
+    {
+        static const FName Name(TEXT("HandTrackingRight"));
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMoreSpecificMotionSourceName(const EControllerHand Hand)
+    {
+        const FName& Name{
+            Hand == EControllerHand::Right
+            ? GetMoreSpecificRightMotionSourceName()
+            : GetMoreSpecificLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMoreSpecificMotionSourceName(const bool bRight)
+    {
+        const FName& Name{
+            bRight
+            ? GetMoreSpecificRightMotionSourceName()
+            : GetMoreSpecificLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetLeftMotionSourceName()
+    {
+        const bool bUseMoreSpecificMotionSourceNames = ShouldUseMoreSpecificMotionSourceNames();
+        const FName& Name{
+            bUseMoreSpecificMotionSourceNames
+            ? GetMoreSpecificLeftMotionSourceName()
+            : GetDefaultLeftMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetRightMotionSourceName()
+    {
+        const bool bUseMoreSpecificMotionSourceNames = ShouldUseMoreSpecificMotionSourceNames();
+        const FName& Name{
+            bUseMoreSpecificMotionSourceNames
+            ? GetMoreSpecificRightMotionSourceName()
+            : GetDefaultRightMotionSourceName()
+        };
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMotionSourceName(const EControllerHand Hand)
+    {
+        const FName& Name{Hand == EControllerHand::Right ? GetRightMotionSourceName() : GetLeftMotionSourceName()};
+        return Name;
+    }
+
+    FORCEINLINE static const FName& GetMotionSourceName(const bool bRight)
+    {
+        const FName& Name{bRight ? GetRightMotionSourceName() : GetLeftMotionSourceName()};
+        return Name;
+    }
+
    static ESGPositionalTrackingHardware GetPositionalTrackingHardware();

    FORCEINLINE static USGHapticGlove* GetGlove(const EControllerHand Hand)
@@ -926,19 +1056,24 @@ void FSGXRTracker::UpdateDeviceLocations(
    LocateInfo.baseSpace = TrackingSpace;
    LocateInfo.time = DisplayTime;

+    const bool bFallbackToHandTrackingIfNoGloveDetected = FImpl::ShouldFallbackToHandTrackingIfNoGloveDetected();
+
    const bool bLeftGloveConnected = FImpl::IsLeftGloveConnected();
    if (bLeftGloveConnected)
    {
        /// NOTE
        /// It's necessary to call the UpdateXRHandTrackingData first for the wrist tracking to work properly when the
        /// motion controllers are not active and we rely on the OpenXRHandTracking data for wrist tracking.
-        /// See the NOTE inside the FSGXRTracker::FImpl::GetWristLocationAndRotation method.
+        /// See the NOTE inside the FSGXRTracker::FImpl::GetWristTransform method.
        Pimpl->UpdateXRHandTrackingData(Pimpl->HandStates[0], LocateInfo);
        Pimpl->UpdateSGHandState(EControllerHand::Left, Pimpl->HandStates[0]);
    }
    else
    {
-        Pimpl->UpdateXRHandTrackingData(Pimpl->HandStates[0], LocateInfo);
+        if (bFallbackToHandTrackingIfNoGloveDetected)
+        {
+            Pimpl->UpdateXRHandTrackingData(Pimpl->HandStates[0], LocateInfo);
+        }
    }

    const bool bRightGloveConnected = FImpl::IsRightGloveConnected();
@@ -953,7 +1088,10 @@ void FSGXRTracker::UpdateDeviceLocations(
    }
    else
    {
-        Pimpl->UpdateXRHandTrackingData(Pimpl->HandStates[1], LocateInfo);
+        if (bFallbackToHandTrackingIfNoGloveDetected)
+        {
+            Pimpl->UpdateXRHandTrackingData(Pimpl->HandStates[1], LocateInfo);
+        }
    }
 }

@@ -984,36 +1122,40 @@ bool FSGXRTracker::GetControllerOrientationAndPosition(const int32 ControllerInd
    {
        FTransform ControllerTransform{FTransform::Identity};

+        const bool bFallbackToHandTrackingIfNoGloveDetected = FImpl::ShouldFallbackToHandTrackingIfNoGloveDetected();
+        if (bFallbackToHandTrackingIfNoGloveDetected)
+        {
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3
-        const FImpl::FSGMotionSourceInfo* KeyPointInfoPtr{Pimpl->MotionSourceToKeypointMap.Find(MotionSource)};
-        if (KeyPointInfoPtr)
+            const FImpl::FSGMotionSourceInfo* KeyPointInfoPtr{Pimpl->MotionSourceToKeypointMap.Find(MotionSource)};
+            if (KeyPointInfoPtr)
 #else   /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
        if (GetControllerTrackingStatus(ControllerIndex, DeviceHand) != ETrackingStatus::NotTracked)
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
-        {
+            {
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3
-            const EHandKeypoint KeyPoint = KeyPointInfoPtr->Key;
-            const bool bIsLeft = KeyPointInfoPtr->Value;
+                const EHandKeypoint KeyPoint = KeyPointInfoPtr->Key;
+                const bool bIsLeft = KeyPointInfoPtr->Value;
 #else   /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
            const bool bIsLeft = DeviceHand == EControllerHand::Left;
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
-            if (bIsLeft)
-            {
+                if (bIsLeft)
+                {
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3
-                ControllerTransform = GetLeftHandState().GetTransform(KeyPoint);
+                    ControllerTransform = GetLeftHandState().GetTransform(KeyPoint);
 #else   /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
                ControllerTransform = GetLeftHandState().GetTransform(EHandKeypoint::Wrist);
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
-                bTracked = !!GetLeftHandState().bReceivedJointPoses;
-            }
-            else
-            {
+                    bTracked = !!GetLeftHandState().bReceivedJointPoses;
+                }
+                else
+                {
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3
-                ControllerTransform = GetRightHandState().GetTransform(KeyPoint);
+                    ControllerTransform = GetRightHandState().GetTransform(KeyPoint);
 #else   /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
                ControllerTransform = GetRightHandState().GetTransform(EHandKeypoint::Wrist);
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
-                bTracked = !!GetRightHandState().bReceivedJointPoses;
+                    bTracked = !!GetRightHandState().bReceivedJointPoses;
+                }
            }
        }

@@ -1026,24 +1168,10 @@ bool FSGXRTracker::GetControllerOrientationAndPosition(const int32 ControllerInd
        {
            static const FName OpenXRName{TEXT("OpenXR")};

+            static constexpr int32 CurrentPlayerControllerIndex = 0;
+
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION <= 2
-            const bool bUseMoreSpecificMotionSourceNames = FImpl::ShouldUseMoreSpecificMotionSourceNames();
-
-            static const FName
-                LeftName(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingLeft") : TEXT("Left"));
-            static const FName RightName(bUseMoreSpecificMotionSourceNames
-                ? TEXT("HandTrackingRight")
-                : TEXT("Right"));
-
-            FName MotionSource;
-            if (DeviceHand == EControllerHand::Left)
-            {
-                MotionSource = LeftName;
-            }
-            else if (DeviceHand == EControllerHand::Right)
-            {
-                MotionSource = RightName;
-            }
+            const FName& MotionSource{FImpl::GetDefaultMotionSourceName(DeviceHand)};
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION <= 2 */

            const TArray<IMotionController*> MotionControllers{
@@ -1064,8 +1192,9 @@ bool FSGXRTracker::GetControllerOrientationAndPosition(const int32 ControllerInd
                    continue;
                }

-                const ETrackingStatus TrackingStatus = MotionController->GetControllerTrackingStatus(0, MotionSource);
-                if (TrackingStatus != ETrackingStatus::Tracked)
+                const ETrackingStatus TrackingStatus = MotionController->GetControllerTrackingStatus(
+                    CurrentPlayerControllerIndex, MotionSource);
+                if (TrackingStatus == ETrackingStatus::NotTracked)
                {
                    continue;
                }
@@ -1110,8 +1239,12 @@ ETrackingStatus FSGXRTracker::GetControllerTrackingStatus(const int32 Controller
            return ETrackingStatus::Tracked;
        }

-        const FSGXRHandState& HandState{bIsLeft ? GetLeftHandState() : GetRightHandState()};
-        return !!HandState.bReceivedJointPoses ? ETrackingStatus::Tracked : ETrackingStatus::NotTracked;
+        const bool bFallbackToHandTrackingIfNoGloveDetected = FImpl::ShouldFallbackToHandTrackingIfNoGloveDetected();
+        if (bFallbackToHandTrackingIfNoGloveDetected)
+        {
+            const FSGXRHandState& HandState{bIsLeft ? GetLeftHandState() : GetRightHandState()};
+            return !!HandState.bReceivedJointPoses ? ETrackingStatus::Tracked : ETrackingStatus::NotTracked;
+        }
 #if ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3
    }
 #endif  /* ENGINE_MAJOR_VERSION >= 5 && ENGINE_MINOR_VERSION >= 3 */
@@ -1129,8 +1262,6 @@ void FSGXRTracker::EnumerateSources(TArray<FMotionControllerSource>& SourcesOut)
 {
    ensureAlwaysMsgf(IsInGameThread(), TEXT("Is not in the game thread!"));

-    const bool bUseMoreSpecificMotionSourceNames = FImpl::ShouldUseMoreSpecificMotionSourceNames();
-
    SourcesOut.Reserve(SourcesOut.Num() + (EHandKeypointCount * 2));

    const UEnum* EnumPtr{
@@ -1138,14 +1269,14 @@ void FSGXRTracker::EnumerateSources(TArray<FMotionControllerSource>& SourcesOut)
    };
    ensureAlwaysMsgf(EnumPtr, TEXT("Failed to find the HMD hand keypoint!"));

-    const FString Left(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingLeft") : TEXT("Left"));
-    const FString Right(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingRight") : TEXT("Right"));
+    const FName& Left(FImpl::GetLeftMotionSourceName());
+    const FName& Right(FImpl::GetRightMotionSourceName());

    for (int32 Keypoint = 0; Keypoint < EHandKeypointCount; Keypoint++)
    {
        const FString EnumString{ParseEOpenXRHandKeypointEnumName(EnumPtr->GetNameByValue(Keypoint)).ToString()};
-        const FString StringLeft{FString::Printf(TEXT("%s%s"), *Left, *EnumString)};
-        const FString StringRight{FString::Printf(TEXT("%s%s"), *Right, *EnumString)};
+        const FString StringLeft{FString::Printf(TEXT("%s%s"), *Left.ToString(), *EnumString)};
+        const FString StringRight{FString::Printf(TEXT("%s%s"), *Right.ToString(), *EnumString)};
        FName SourceL(*(StringLeft));
        FName SourceR(*(StringRight));
        SourcesOut.Add(MoveTemp(SourceL));
@@ -1345,7 +1476,6 @@ void FSGXRTracker::FImpl::BuildMotionSourceToKeypointMap()
        return;
    }

-    const bool bUseMoreSpecificMotionSourceNames = ShouldUseMoreSpecificMotionSourceNames();
    const bool bSupportLegacyControllerMotionSources = ShouldSupportLegacyControllerMotionSources();

    // There is a motion source that corresponds to each hand keypoint of the form [Left|Right][Keypoint].
@@ -1360,24 +1490,28 @@ void FSGXRTracker::FImpl::BuildMotionSourceToKeypointMap()

    ensureAlwaysMsgf(IsInGameThread(), TEXT("Is not in the game thread!"));

-    const FString Left(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingLeft") : TEXT("Left"));
-    const FString Right(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingRight") : TEXT("Right"));
+    const FName& Left(GetLeftMotionSourceName());
+    const FName& Right(GetRightMotionSourceName());

    for (int64 e = 0; e < EHandKeypointCount; ++e)
    {
        const EHandKeypoint EnumValue = static_cast<EHandKeypoint>(e);
        const FString EnumName{EnumPtr->GetNameStringByValue(e)};

-        FName LeftName(Left + EnumName);
-        FName RightName(Right + EnumName);
+        FName LeftName(Left.ToString() + EnumName);
+        FName RightName(Right.ToString() + EnumName);
        MotionSourceToKeypointMap.Add(MoveTemp(LeftName), FSGMotionSourceInfo(EnumValue, true));
        MotionSourceToKeypointMap.Add(MoveTemp(RightName), FSGMotionSourceInfo(EnumValue, false));
    }

    if (bSupportLegacyControllerMotionSources)
    {
-        MotionSourceToKeypointMap.Add(FName("Left"), FSGMotionSourceInfo(EHandKeypoint::Wrist, true));
-        MotionSourceToKeypointMap.Add(FName("Right"), FSGMotionSourceInfo(EHandKeypoint::Wrist, false));
+        const FName& LeftLegacyControllerMotionSourceName{GetLegacyControllerLeftMotionSourceName()};
+        const FName& RightLegacyControllerMotionSourceName{GetLegacyControllerRightMotionSourceName()};
+        MotionSourceToKeypointMap.Add(
+            LeftLegacyControllerMotionSourceName, FSGMotionSourceInfo(EHandKeypoint::Wrist, true));
+        MotionSourceToKeypointMap.Add(
+            RightLegacyControllerMotionSourceName, FSGMotionSourceInfo(EHandKeypoint::Wrist, false));
    }
 }

@@ -1526,23 +1660,11 @@ bool FSGXRTracker::FImpl::GetWristTransform(
    static const FName OpenXRName{TEXT("OpenXR")};
    static const FName OpenXRHandTrackingName{TEXT("OpenXRHandTracking")};

-    const bool bUseMoreSpecificMotionSourceNames = ShouldUseMoreSpecificMotionSourceNames();
-
-    static const FName LeftName(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingLeft") : TEXT("Left"));
-    static const FName RightName(bUseMoreSpecificMotionSourceNames ? TEXT("HandTrackingRight") : TEXT("Right"));
-
-    FName MotionSource;
-    if (DeviceHand == EControllerHand::Left)
-    {
-        MotionSource = LeftName;
-    }
-    else if (DeviceHand == EControllerHand::Right)
-    {
-        MotionSource = RightName;
-    }
-
+    const FName& MotionSource{GetMotionSourceName(DeviceHand)};
    static constexpr int32 CurrentPlayerControllerIndex = 0;

+    const bool bFallbackToHandTrackingIfNoGloveDetected = ShouldFallbackToHandTrackingIfNoGloveDetected();
+
    const TArray<IMotionController*> MotionControllers{IModularFeatures::Get().GetModularFeatureImplementations<
        IMotionController>(IMotionController::GetModularFeatureName())};

@@ -1553,25 +1675,28 @@ bool FSGXRTracker::FImpl::GetWristTransform(
            continue;
        }

-        ETrackingStatus TrackingStatus = MotionController->GetControllerTrackingStatus(
+        const ETrackingStatus TrackingStatus = MotionController->GetControllerTrackingStatus(
            CurrentPlayerControllerIndex, MotionSource);
-        if (TrackingStatus != ETrackingStatus::Tracked)
+        if (TrackingStatus == ETrackingStatus::NotTracked)
        {
            continue;
        }

-        FName DeviceTypeName{MotionController->GetMotionControllerDeviceTypeName()};
+        const FName DeviceTypeName{MotionController->GetMotionControllerDeviceTypeName()};

        /// NOTE
-        /// We have to always check the wrist tracking data from the OpenXRHandTracking system instead of OpenXR,
-        /// otherwise even if the motion controllers become deactivated their tracking status sometimes mysteriously
-        /// reported as Tracked.
+        // We have to always check the wrist tracking data from the OpenXRHandTracking system instead of OpenXR,
+        // otherwise even if the motion controllers become deactivated their tracking status sometimes mysteriously
+        // reported as Tracked.

-        if (DeviceTypeName == OpenXRHandTrackingName)
+        if (bFallbackToHandTrackingIfNoGloveDetected)
        {
-            bOutMotionSourceHandTracking = true;
-            const bool bResult = GetOpenXRHandTrackingWristTransform(DeviceHand, OutTransform);
-            return bResult;
+            if (DeviceTypeName == OpenXRHandTrackingName)
+            {
+                bOutMotionSourceHandTracking = true;
+                const bool bResult = GetOpenXRHandTrackingWristTransform(DeviceHand, OutTransform);
+                return bResult;
+            }
        }

        if (DeviceTypeName == OpenXRName)
@@ -1582,6 +1707,20 @@ bool FSGXRTracker::FImpl::GetWristTransform(
        }
    }

+    /// WORKAROUND
+    // Sometimes the motion controller array does not contain a OpenXRHandTracking though it's being tracked! So, as a
+    // last resort we always attempt at retrieving the OpenXRHandTracking wrist transform in case all other measures we
+    // have taken has already failed!
+    if (bFallbackToHandTrackingIfNoGloveDetected)
+    {
+        const bool bResult = GetOpenXRHandTrackingWristTransform(DeviceHand, OutTransform);
+        if (bResult)
+        {
+            bOutMotionSourceHandTracking = true;
+        }
+        return bResult;
+    }
+
    return false;
 }

@@ -2039,10 +2178,13 @@ bool FSGXRTracker::FImpl::UpdateSGHandState(const EControllerHand DeviceHand, FS

    FTransform WristTransform;
    bool bMotionSourceHandTracking = false;
-    if (!GetWristTransform(DeviceHand, WristTransform, bMotionSourceHandTracking))
-    {
-        return false;
-    }
+    const bool bGotWristTransform = GetWristTransform(DeviceHand, WristTransform, bMotionSourceHandTracking);
+
+    /// NOTE
+    // Even if we fail to get the wrist transform, the SenseGlove data needs to make it to the FXRMotionControllerData!
+    // So, we just ignore the bGotWristTransform results! Otherwise, even if bFallbackToHandTrackingIfNoGloveDetected
+    // is disabled it will output the hand-tracking data.
+    (void) bGotWristTransform;

    if (!UpdateSGWristJointData(OutHandState, WristTransform, bMotionSourceHandTracking))
    {