attempt at calculating a hand model and feeding it to get hand pose

Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp

@@ -204,6 +204,32 @@ struct FSGXRTracker::FImpl
         return IsValid(GloveTracker) ? GloveTracker->IsAnyGloveConnected() : false;
     }
 
+    static TArray<float> GetFingerLengths(
+        bool bRight,
+        EHandKeypoint Joint1st, EHandKeypoint Joint2nd, EHandKeypoint Joint3rd,
+        float DistalPhalanxLength);
+
+    static TArray<TArray<float>> GetFingerLengths(bool bRight);
+    static TArray<FVector> GetFingerStartPositions(bool bRight);
+    static TArray<FQuat> GetFingerStartRotations(bool bRight);
+    static USGBasicHandModel* GetHandModel(bool bRight);
+
+    FORCEINLINE static USGBasicHandModel* GetHandModel(const EControllerHand Hand)
+    {
+        const bool bRight = Hand == EControllerHand::Right;
+        USGBasicHandModel* HandModel{GetHandModel(bRight)};
+        return HandModel;
+    }
+
+    static USGHandPose* GetHandPose(bool bRight);
+
+    FORCEINLINE static USGHandPose* GetHandPose(const EControllerHand Hand)
+    {
+        const bool bRight = Hand == EControllerHand::Right;
+        USGHandPose* HandPose{GetHandPose(bRight)};
+        return HandPose;
+    }
+
     FORCEINLINE static const FName& GetLeftMeshBoneName(const int32 JointIndex)
     {
         const FName& Name{GetVirtualHandTrackingSettings().LeftHandBoneNames[JointIndex]};
@@ -560,6 +586,139 @@ ESGPositionalTrackingHardware FSGXRTracker::FImpl::GetPositionalTrackingHardware
     return TrackerHardware;
 }
 
+TArray<float> FSGXRTracker::FImpl::GetFingerLengths(
+    const bool bRight,
+    const EHandKeypoint Joint1st, const EHandKeypoint Joint2nd, const EHandKeypoint Joint3rd,
+    const float DistalPhalanxLength)
+{
+    const FTransform& Joint1stTransform{GetMeshBoneRefTransform(bRight, Joint1st)};
+    const FVector& Joint1stLocation{Joint1stTransform.GetLocation()};
+    const FTransform& Joint2ndTransform{GetMeshBoneRefTransform(bRight, Joint2nd)};
+    const FVector& Joint2ndLocation{Joint2ndTransform.GetLocation()};
+    const FTransform& Joint3rdTransform{GetMeshBoneRefTransform(bRight, Joint3rd)};
+    const FVector& Joint3rdLocation{Joint3rdTransform.GetLocation()};
+
+    const float PhalanxLength1st = static_cast<float>(FVector::Dist(Joint1stLocation, Joint2ndLocation));
+    const float PhalanxLength2nd = static_cast<float>(FVector::Dist(Joint2ndLocation, Joint3rdLocation));
+
+    const TArray<float> Lengths{PhalanxLength1st, PhalanxLength2nd, DistalPhalanxLength};
+    return Lengths;
+}
+
+TArray<TArray<float>> FSGXRTracker::FImpl::GetFingerLengths(const bool bRight)
+{
+    const FSGVirtualHandTrackingSettings& Settings{GetVirtualHandTrackingSettings()};
+
+    const TArray<TArray<float>> Lengths{
+        GetFingerLengths(bRight,
+                         EHandKeypoint::ThumbMetacarpal,
+                         EHandKeypoint::ThumbProximal,
+                         EHandKeypoint::ThumbDistal,
+                         Settings.ThumbDistalPhalanxLength
+            ),
+        GetFingerLengths(bRight,
+                         EHandKeypoint::IndexProximal,
+                         EHandKeypoint::IndexIntermediate,
+                         EHandKeypoint::IndexDistal,
+                         Settings.IndexDistalPhalanxLength
+            ),
+        GetFingerLengths(bRight,
+                         EHandKeypoint::MiddleProximal,
+                         EHandKeypoint::MiddleIntermediate,
+                         EHandKeypoint::MiddleDistal,
+                         Settings.MiddleDistalPhalanxLength
+            ),
+        GetFingerLengths(bRight,
+                         EHandKeypoint::RingProximal,
+                         EHandKeypoint::RingIntermediate,
+                         EHandKeypoint::RingDistal,
+                         Settings.RingDistalPhalanxLength
+            ),
+        GetFingerLengths(bRight,
+                         EHandKeypoint::LittleProximal,
+                         EHandKeypoint::LittleIntermediate,
+                         EHandKeypoint::LittleDistal,
+                         Settings.LittleDistalPhalanxLength
+            ),
+    };
+
+    return Lengths;
+}
+
+TArray<FVector> FSGXRTracker::FImpl::GetFingerStartPositions(const bool bRight)
+{
+    const FTransform& ThumbStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal)};
+    const FTransform& IndexStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal)};
+    const FTransform& MiddleStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal)};
+    const FTransform& RingStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal)};
+    const FTransform& LittleStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal)};
+
+    const TArray<FVector> StartPositions{
+        ThumbStartPosition.GetLocation(),
+        IndexStartPosition.GetLocation(),
+        MiddleStartPosition.GetLocation(),
+        RingStartPosition.GetLocation(),
+        LittleStartPosition.GetLocation(),
+    };
+
+    return StartPositions;
+}
+
+TArray<FQuat> FSGXRTracker::FImpl::GetFingerStartRotations(const bool bRight)
+{
+    const FTransform& ThumbStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal)};
+    const FTransform& IndexStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal)};
+    const FTransform& MiddleStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal)};
+    const FTransform& RingStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal)};
+    const FTransform& LittleStartPosition{GetMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal)};
+
+    const TArray<FQuat> StartRotations{
+        ThumbStartPosition.GetRotation(),
+        IndexStartPosition.GetRotation(),
+        MiddleStartPosition.GetRotation(),
+        RingStartPosition.GetRotation(),
+        LittleStartPosition.GetRotation(),
+    };
+
+    return StartRotations;
+}
+
+USGBasicHandModel* FSGXRTracker::FImpl::GetHandModel(const bool bRight)
+{
+    const TArray<TArray<float>> FingerLengths{GetFingerLengths(bRight)};
+    const TArray<FVector> StartPositions{GetFingerStartPositions(bRight)};
+    const TArray<FQuat> StartRotations{GetFingerStartRotations(bRight)};
+
+    UWorld* World{GetWorld()};
+
+    USGBasicHandModel* HandModel{
+        USGBasicHandModel::NewBasicHandModel(World, bRight, FingerLengths, StartPositions, StartRotations)
+    };
+
+    return HandModel;
+}
+
+USGHandPose* FSGXRTracker::FImpl::GetHandPose(const bool bRight)
+{
+    const USGHapticGlove* Glove{GetGlove(bRight)};
+    if (!IsValid(Glove))
+    {
+        return nullptr;
+    }
+
+    UWorld* World{GetWorld()};
+    const USGBasicHandModel* HandModel{GetHandModel(bRight)};
+    USGHandPose* HandPose{nullptr};
+
+    const bool bGotHandPose = Glove->GetHandPose(World, HandModel, HandPose);
+    if (!bGotHandPose || !IsValid(HandPose))
+    {
+        return nullptr;
+    }
+
+    return HandPose;
+}
+
 const FTransform& FSGXRTracker::FImpl::GetRawMeshBoneRefTransform(const bool bRight, const FName& BoneName)
 {
     const TSoftObjectPtr<USkeletalMesh>& HandMesh{
@@ -1913,17 +2072,8 @@ bool FSGXRTracker::FImpl::UpdateSGLittleJointData(
 
 bool FSGXRTracker::FImpl::UpdateSGHandState(const EControllerHand DeviceHand, FSGXRHandState& OutHandState) const
 {
-    USGHapticGlove* Glove{GetGlove(DeviceHand)};
-
-    if (!IsValid(Glove))
-    {
-        return false;
-    }
-
-    USGHandPose* HandPose = nullptr;
-    const bool bGotHandPose = Glove->GetHandPose(FImpl::GetWorld(), HandPose);
-
-    if (!bGotHandPose || !IsValid(HandPose))
+    USGHandPose* HandPose{FImpl::GetHandPose(DeviceHand)};
+    if (!IsValid(HandPose))
     {
         return false;
     }