implement our own GetMotionControllerData instead of using the one from the XRTrackingSystem as it only picks up the first hand tracking plugin in order to make sure at least internally we use the hand-tracking data from senseglove

2024-08-16 18:00:56 +02:00
parent 54481effbd
commit d0b2ec380e
8 changed files with 354 additions and 20 deletions
@@ -41,6 +41,7 @@
 #include "Engine/SkeletalMesh.h"
 #include "Engine/World.h"
 #include "GameFramework/WorldSettings.h"
+#include "IOpenXRHMD.h"
 #include "IOpenXRHMDModule.h"
 #include "IXRTrackingSystem.h"
 #include "OpenXRCore.h"
@@ -972,6 +973,192 @@ FTransform FSGXRTracker::FImpl::GetMeshFingerStartBoneRefTransform(
    return Transform;
 }

+bool FSGXRTracker::GetMotionControllerData(
+    UObject* WorldContextObject, const EControllerHand Hand, FXRMotionControllerData& OutMotionControllerData)
+{
+    OutMotionControllerData.DeviceName = NAME_None;
+    OutMotionControllerData.ApplicationInstanceID = FApp::GetInstanceId();
+    OutMotionControllerData.DeviceVisualType = EXRVisualType::Controller;
+    OutMotionControllerData.TrackingStatus = ETrackingStatus::NotTracked;
+    OutMotionControllerData.HandIndex = Hand;
+    OutMotionControllerData.bValid = false;
+
+    if (!IsValid(WorldContextObject))
+    {
+        return false;
+    }
+
+    IXRTrackingSystem* XRTrackingSystem{GEngine ? GEngine->XRSystem.Get() : nullptr};
+    if (!XRTrackingSystem)
+    {
+        return false;
+    }
+
+    IOpenXRHMD* OpenXRHMD{XRTrackingSystem->GetIOpenXRHMD()};
+    if (!OpenXRHMD)
+    {
+        return false;
+    }
+
+    TArray<int32> Devices;
+    const int32 HandIndex = static_cast<int32>(Hand);
+    const bool bGotDevices =
+        XRTrackingSystem->EnumerateTrackedDevices(Devices, EXRTrackedDeviceType::Controller);
+    if (bGotDevices && Devices.IsValidIndex(HandIndex))
+    {
+        const XrSession Session{OpenXRHMD->GetSession()};
+        if (Session)
+        {
+            XrInteractionProfileState Profile = {XR_TYPE_INTERACTION_PROFILE_STATE};
+            const XrPath TopLevelUserPath{OpenXRHMD->GetTrackedDevicePath(Devices[HandIndex])};
+            const XrResult CurrentInteractionProfile{
+                xrGetCurrentInteractionProfile(Session, TopLevelUserPath, &Profile)
+            };
+            if (XR_SUCCEEDED(CurrentInteractionProfile) && Profile.interactionProfile != XR_NULL_PATH)
+            {
+                OutMotionControllerData.DeviceName = FOpenXRPath(Profile.interactionProfile);
+            }
+        }
+    }
+
+    static const FName HandTrackerName{"OpenXRHandTracking"};
+    TArray<IHandTracker*> HandTrackers = IModularFeatures::Get().GetModularFeatureImplementations<IHandTracker>(
+        IHandTracker::GetModularFeatureName());
+    IHandTracker* HandTracker{nullptr};
+    for (IHandTracker* HandTrackerIterator: HandTrackers)
+    {
+        if (!HandTrackerIterator)
+        {
+            continue;
+        }
+
+        const FName DeviceTypeName{HandTrackerIterator->GetHandTrackerDeviceTypeName()};
+        if (DeviceTypeName != HandTrackerName)
+        {
+            continue;
+        }
+
+        FSGXRTracker* SGXRTracker{static_cast<FSGXRTracker*>(HandTrackerIterator)};
+        if (!SGXRTracker)
+        {
+            continue;
+        }
+
+        HandTracker = SGXRTracker;
+        break;
+    }
+
+    if (Hand == EControllerHand::Left || Hand == EControllerHand::Right)
+    {
+        static const FName MotionControllerName("OpenXR");
+        TArray<IMotionController*> MotionControllers{
+            IModularFeatures::Get().GetModularFeatureImplementations<IMotionController>(
+                IMotionController::GetModularFeatureName())
+        };
+        IMotionController* MotionController{nullptr};
+        for (IMotionController* MotionControllerIterator: MotionControllers)
+        {
+            if (!MotionControllerIterator)
+            {
+                continue;
+            }
+
+            const FName DeviceTypeName{MotionControllerIterator->GetMotionControllerDeviceTypeName()};
+            if (DeviceTypeName != MotionControllerName)
+            {
+                continue;
+            }
+
+            MotionController = MotionControllerIterator;
+            break;
+        }
+
+        const float WorldToMeters = XRTrackingSystem->GetWorldToMetersScale();
+        if (MotionController)
+        {
+            const FTransform TrackingToWorld{XRTrackingSystem->GetTrackingToWorldTransform()};
+
+            {
+                const FName AimSource{Hand == EControllerHand::Left ? FName{TEXT("LeftAim")} : FName{TEXT("RightAim")}};
+                FRotator AimSourceRotation;
+                FVector AimSourcePosition;
+                const bool bGotAimOrientationAndPosition =
+                    MotionController->GetControllerOrientationAndPosition(
+                        0, AimSource, AimSourceRotation, AimSourcePosition, WorldToMeters);
+                if (bGotAimOrientationAndPosition)
+                {
+                    OutMotionControllerData.AimPosition = TrackingToWorld.TransformPosition(AimSourcePosition);
+                    OutMotionControllerData.AimRotation =
+                        TrackingToWorld.TransformRotation(AimSourceRotation.Quaternion());
+                }
+                OutMotionControllerData.bValid |= bGotAimOrientationAndPosition;
+            }
+
+            const FName GripSource{Hand == EControllerHand::Left ? FName{TEXT("LeftGrip")} : FName{TEXT("RightGrip")}};
+            {
+                FRotator GripRotation;
+                FVector GripPosition;
+                const bool bGotGripSourceOrientationAndPosition =
+                    MotionController->GetControllerOrientationAndPosition(
+                        0, GripSource, GripRotation, GripPosition, WorldToMeters);
+                if (bGotGripSourceOrientationAndPosition)
+                {
+                    OutMotionControllerData.GripPosition = TrackingToWorld.TransformPosition(GripPosition);
+                    OutMotionControllerData.GripRotation =
+                        TrackingToWorld.TransformRotation(GripRotation.Quaternion());
+                }
+                OutMotionControllerData.bValid |= bGotGripSourceOrientationAndPosition;
+            }
+
+            {
+                const FName PalmSource{
+                    Hand == EControllerHand::Left ? FName{TEXT("LeftPalm")} : FName{TEXT("RightPalm")}
+                };
+                FRotator PalmRotation;
+                FVector PalmPosition;
+                const bool bGotPalmOrientationAndPosition = MotionController->GetControllerOrientationAndPosition(
+                    0, PalmSource, PalmRotation, PalmPosition, WorldToMeters);
+                if (bGotPalmOrientationAndPosition)
+                {
+                    OutMotionControllerData.GripPosition = TrackingToWorld.TransformPosition(PalmPosition);
+                    OutMotionControllerData.GripRotation =
+                        TrackingToWorld.TransformRotation(PalmRotation.Quaternion());
+                }
+                OutMotionControllerData.bValid |= bGotPalmOrientationAndPosition;
+            }
+
+            OutMotionControllerData.TrackingStatus = MotionController->GetControllerTrackingStatus(0, GripSource);
+        }
+
+        const bool bHandTrackingStateValid = HandTracker && HandTracker->IsHandTrackingStateValid();
+        if (bHandTrackingStateValid)
+        {
+            OutMotionControllerData.DeviceVisualType = EXRVisualType::Hand;
+
+            OutMotionControllerData.bValid = HandTracker->GetAllKeypointStates(
+                Hand, OutMotionControllerData.HandKeyPositions, OutMotionControllerData.HandKeyRotations,
+                OutMotionControllerData.HandKeyRadii);
+
+            if (!ensureAlwaysMsgf(
+                !OutMotionControllerData.bValid
+                || (OutMotionControllerData.HandKeyPositions.Num() == EHandKeypointCount
+                    && OutMotionControllerData.HandKeyRotations.Num() == EHandKeypointCount
+                    && OutMotionControllerData.HandKeyRadii.Num() == EHandKeypointCount),
+                TEXT("Corrupted OutMotionControllerData!")))
+            {
+                OutMotionControllerData.bValid = false;
+                return false;
+            }
+        }
+    }
+
+    /// TODO
+    // This is reportedly a WMR-specific convenience function for rapid prototyping. Not sure if it's useful for OpenXR.
+    OutMotionControllerData.bIsGrasped = false;
+
+    return OutMotionControllerData.bValid;
+}
+
 FSGXRTracker::FSGXRTracker(const TSharedRef<FGenericApplicationMessageHandler>& InMessageHandler)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
 {