From da8289f2b39e908259fda68ad913992b6a54bcc6 Mon Sep 17 00:00:00 2001 From: Mamadou Babaei Date: Wed, 22 May 2024 17:25:56 +0200 Subject: [PATCH] somewhat unfinished refactored sg bone calculations --- .../Private/SGTracking/SGXRTracker.cpp | 238 +++++++++++------- 1 file changed, 149 insertions(+), 89 deletions(-) diff --git a/Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp b/Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp index b60af50d..07370601 100644 --- a/Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp +++ b/Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp @@ -206,7 +206,7 @@ struct FSGXRTracker::FImpl static TArray GetFingerLengths( bool bRight, - EHandKeypoint Joint1st, EHandKeypoint Joint2nd, EHandKeypoint Joint3rd, + EHandKeypoint Joint2nd, EHandKeypoint Joint3rd, float DistalPhalanxLength); static TArray> GetFingerLengths(bool bRight); @@ -320,44 +320,44 @@ struct FSGXRTracker::FImpl return Transform; } - static FTransform GetOpenXRMeshMetacarpalBoneRefTransform(bool bRight, const FName& BoneName); + static FTransform GetOpenXRMeshBoneRefTransform(bool bRight, const FName& BoneName); - FORCEINLINE static FTransform GetOpenXRMeshMetacarpalBoneRefTransform(const bool bRight, const int32 JointIndex) + FORCEINLINE static FTransform GetOpenXRMeshBoneRefTransform(const bool bRight, const int32 JointIndex) { const FName& BoneName{GetMeshBoneName(bRight, JointIndex)}; - const FTransform Transform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, BoneName)}; + const FTransform Transform{GetOpenXRMeshBoneRefTransform(bRight, BoneName)}; return Transform; } - FORCEINLINE static FTransform GetOpenXRMeshMetacarpalBoneRefTransform( + FORCEINLINE static FTransform GetOpenXRMeshBoneRefTransform( const bool bRight, const EHandKeypoint Keypoint) { const FName& BoneName{GetMeshBoneName(bRight, Keypoint)}; - const FTransform Transform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, BoneName)}; + const FTransform Transform{GetOpenXRMeshBoneRefTransform(bRight, BoneName)}; return Transform; } - FORCEINLINE static FTransform GetOpenXRMeshMetacarpalBoneRefTransform( + FORCEINLINE static FTransform GetOpenXRMeshBoneRefTransform( const EControllerHand Hand, const FName& BoneName) { const bool bRight = Hand == EControllerHand::Right; - const FTransform Transform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, BoneName)}; + const FTransform Transform{GetOpenXRMeshBoneRefTransform(bRight, BoneName)}; return Transform; } - FORCEINLINE static FTransform GetOpenXRMeshMetacarpalBoneRefTransform( + FORCEINLINE static FTransform GetOpenXRMeshBoneRefTransform( const EControllerHand Hand, const int32 JointIndex) { const bool bRight = Hand == EControllerHand::Right; - const FTransform Transform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, JointIndex)}; + const FTransform Transform{GetOpenXRMeshBoneRefTransform(bRight, JointIndex)}; return Transform; } - FORCEINLINE static FTransform GetOpenXRMeshMetacarpalBoneRefTransform( + FORCEINLINE static FTransform GetOpenXRMeshBoneRefTransform( const EControllerHand Hand, const EHandKeypoint Keypoint) { const bool bRight = Hand == EControllerHand::Right; - const FTransform Transform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, Keypoint)}; + const FTransform Transform{GetOpenXRMeshBoneRefTransform(bRight, Keypoint)}; return Transform; } @@ -634,18 +634,16 @@ ESGPositionalTrackingHardware FSGXRTracker::FImpl::GetPositionalTrackingHardware TArray FSGXRTracker::FImpl::GetFingerLengths( const bool bRight, - const EHandKeypoint Joint1st, const EHandKeypoint Joint2nd, const EHandKeypoint Joint3rd, + const EHandKeypoint Joint2nd, const EHandKeypoint Joint3rd, const float DistalPhalanxLength) { - const FTransform& Joint1stTransform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, Joint1st)}; - const FVector& Joint1stLocation{Joint1stTransform.GetLocation()}; - const FTransform& Joint2ndTransform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, Joint2nd)}; + const FTransform& Joint2ndTransform{GetOpenXRMeshBoneRefTransform(bRight, Joint2nd)}; const FVector& Joint2ndLocation{Joint2ndTransform.GetLocation()}; - const FTransform& Joint3rdTransform{GetOpenXRMeshMetacarpalBoneRefTransform(bRight, Joint3rd)}; + const FTransform& Joint3rdTransform{GetOpenXRMeshBoneRefTransform(bRight, Joint3rd)}; const FVector& Joint3rdLocation{Joint3rdTransform.GetLocation()}; - const float PhalanxLength1st = static_cast(FVector::Dist(Joint1stLocation, Joint2ndLocation)); - const float PhalanxLength2nd = static_cast(FVector::Dist(Joint2ndLocation, Joint3rdLocation)); + const float PhalanxLength1st = static_cast(Joint2ndLocation.Size()); + const float PhalanxLength2nd = static_cast(Joint3rdLocation.Size()); const TArray Lengths{PhalanxLength1st, PhalanxLength2nd, DistalPhalanxLength}; return Lengths; @@ -657,31 +655,26 @@ TArray> FSGXRTracker::FImpl::GetFingerLengths(const bool bRight) const TArray> 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 @@ -695,47 +688,64 @@ TArray FSGXRTracker::FImpl::GetFingerStartPositions(const bool bRight) { const FVector WristPosition{GetRawMeshBoneRefTransform(bRight, EHandKeypoint::Wrist).GetLocation()}; - const FVector ThumbStartPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal).GetLocation() - + WristPosition + const FQuat RootRotationCorrection{ + GetVirtualHandTrackingSettings().HandMeshToOpenXRRootBoneRotationCorrection.Quaternion() + }; + FTransform RootBoneRefTransform{GetRawMeshBoneRefTransform(bRight, EHandKeypoint::Wrist)}; + RootBoneRefTransform.SetRotation(RootRotationCorrection * RootBoneRefTransform.GetRotation()); + + + FVector ThumbStartPosition{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal).GetLocation() }; - const FVector IndexMetacarpalPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::IndexMetacarpal).GetLocation() - + WristPosition + FTransform IndexMetacarpalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexMetacarpal) }; - const FVector IndexStartPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::IndexProximal).GetLocation() - + IndexMetacarpalPosition + FTransform IndexStartTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal) }; - const FVector MiddleMetacarpalPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::MiddleMetacarpal).GetLocation() - + WristPosition + FVector IndexStartPositionUncorrected = IndexMetacarpalTransform.GetLocation() + (IndexMetacarpalTransform. + GetRotation() * IndexStartTransform.GetLocation()); + FVector IndexStartPosition = RootBoneRefTransform.GetRotation() * IndexStartPositionUncorrected; + + FTransform MiddleMetacarpalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::MiddleMetacarpal) }; - const FVector MiddleStartPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::MiddleProximal).GetLocation() - + MiddleMetacarpalPosition + FTransform MiddleStartTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal) }; - const FVector RingMetacarpalPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::RingMetacarpal).GetLocation() - + WristPosition + FVector MiddleStartPositionUncorrected = MiddleMetacarpalTransform.GetLocation() + (MiddleMetacarpalTransform. + GetRotation() * MiddleStartTransform.GetLocation()); + FVector MiddleStartPosition = RootBoneRefTransform.GetRotation() * MiddleStartPositionUncorrected; + + + FTransform RingMetacarpalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::RingMetacarpal) }; - const FVector RingStartPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::RingProximal).GetLocation() - + RingMetacarpalPosition + FTransform RingStartTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal) }; - const FVector LittleMetacarpalPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::LittleMetacarpal).GetLocation() - + WristPosition + FVector RingStartPositionUncorrected = RingMetacarpalTransform.GetLocation() + (RingMetacarpalTransform. + GetRotation() * RingStartTransform.GetLocation()); + FVector RingStartPosition = RootBoneRefTransform.GetRotation() * RingStartPositionUncorrected; + + + FTransform LittleMetacarpalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::LittleMetacarpal) }; - const FVector LittleStartPosition{ - GetOpenXRMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::LittleProximal).GetLocation() - + LittleMetacarpalPosition + FTransform LittleStartTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal) }; + FVector LittleStartPositionUncorrected = LittleMetacarpalTransform.GetLocation() + (LittleMetacarpalTransform. + GetRotation() * LittleStartTransform.GetLocation()); + FVector LittleStartPosition = RootBoneRefTransform.GetRotation() * LittleStartPositionUncorrected; + + const TArray StartPositions{ ThumbStartPosition, IndexStartPosition, @@ -749,44 +759,83 @@ TArray FSGXRTracker::FImpl::GetFingerStartPositions(const bool bRight) TArray FSGXRTracker::FImpl::GetFingerStartRotations(const bool bRight) { + // const FQuat ThumbStartRotation{ + // GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal).GetRotation() + // }; + // + // const FQuat IndexMetacarpalRotation{ + // GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::IndexMetacarpal).GetRotation() + // }; + // const FQuat IndexStartRotation{ + // GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal).GetRotation() * IndexMetacarpalRotation + // }; + // + // const FQuat MiddleMetacarpalRotation{ + // GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::MiddleMetacarpal).GetRotation() + // }; + // const FQuat MiddleStartRotation{ + // GetRawMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal).GetRotation() * MiddleMetacarpalRotation + // }; + // + // const FQuat RingMetacarpalRotation{ + // GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::RingMetacarpal).GetRotation() + // }; + // const FQuat RingStartRotation{ + // GetRawMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal).GetRotation() * RingMetacarpalRotation + // }; + // + // const FQuat LittleMetacarpalRotation{ + // GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::LittleMetacarpal).GetRotation() + // }; + // const FQuat LittleStartRotation{ + // GetRawMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal).GetRotation() * LittleMetacarpalRotation + // }; + const FQuat ThumbStartRotation{ - GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal).GetRotation() + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::ThumbMetacarpal).GetRotation() }; - const FQuat IndexMetacarpalRotation{ - GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::IndexMetacarpal).GetRotation() + const FQuat RootRotationCorrection{ + GetVirtualHandTrackingSettings().HandMeshToSenseGloveRootBoneRotationCorrection.Quaternion() }; - const FQuat IndexStartRotation{ - GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal).GetRotation() * IndexMetacarpalRotation + FTransform RootBoneRefTransform{GetRawMeshBoneRefTransform(bRight, EHandKeypoint::Wrist)}; + RootBoneRefTransform.SetRotation(RootRotationCorrection * RootBoneRefTransform.GetRotation()); + + const FTransform IndexMetacarpalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexMetacarpal) + }; + const FTransform IndexProximalTransform{ + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::IndexProximal) }; - const FQuat MiddleMetacarpalRotation{ - GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::MiddleMetacarpal).GetRotation() - }; + const FQuat IndexStartRotation{IndexProximalTransform.GetRotation() * RootBoneRefTransform.GetRotation()}; + + + const FQuat MiddleStartRotation{ - GetRawMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal).GetRotation() * MiddleMetacarpalRotation + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::MiddleProximal).GetRotation() }; - const FQuat RingMetacarpalRotation{ - GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::RingMetacarpal).GetRotation() - }; const FQuat RingStartRotation{ - GetRawMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal).GetRotation() * RingMetacarpalRotation + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::RingProximal).GetRotation() }; - const FQuat LittleMetacarpalRotation{ - GetSGMeshMetacarpalBoneRefTransform(bRight, EHandKeypoint::LittleMetacarpal).GetRotation() - }; const FQuat LittleStartRotation{ - GetRawMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal).GetRotation() * LittleMetacarpalRotation + GetRawMeshBoneRefTransform(bRight, EHandKeypoint::LittleProximal).GetRotation() }; + const TArray StartRotations{ - ThumbStartRotation, - IndexStartRotation, - MiddleStartRotation, - RingStartRotation, - LittleStartRotation, + // ThumbStartRotation, + // IndexStartRotation, + // MiddleStartRotation, + // RingStartRotation, + // LittleStartRotation, + FQuat::Identity, + FQuat::Identity, + FQuat::Identity, + FQuat::Identity, + FQuat::Identity, }; return StartRotations; @@ -870,7 +919,8 @@ const FTransform& FSGXRTracker::FImpl::GetRawMeshBoneRefTransform(const bool bRi return Transform; } -FTransform FSGXRTracker::FImpl::GetOpenXRMeshMetacarpalBoneRefTransform(const bool bRight, const FName& BoneName) +FTransform FSGXRTracker::FImpl::GetOpenXRMeshBoneRefTransform( + const bool bRight, const FName& BoneName) { const FQuat RootRotationCorrection{ GetVirtualHandTrackingSettings().HandMeshToOpenXRRootBoneRotationCorrection.Quaternion() @@ -1802,9 +1852,11 @@ bool FSGXRTracker::FImpl::UpdateSGPalmJointData( return false; } - const FTransform JointTransform{GetOpenXRMeshMetacarpalBoneRefTransform(DeviceHand, EHandKeypoint::Palm)}; - FVector JointLocation{JointTransform.GetLocation()}; - FQuat JointRotation{JointTransform.GetRotation()}; + const FTransform JointTransform{GetOpenXRMeshBoneRefTransform(DeviceHand, EHandKeypoint::Palm)}; + // FVector JointLocation{JointTransform.GetLocation()}; + // FQuat JointRotation{JointTransform.GetRotation()}; + FVector JointLocation{4.967f, 0.090f, -0.132f}; + FQuat JointRotation{0.000000000f, 0.000000000f, 0.000000000f, 0.999999829f}; if (!UpdateSGJointData(OutHandState, EHandKeypoint::Palm, MoveTemp(JointLocation), MoveTemp(JointRotation), WristLocation, WristRotation, @@ -1893,10 +1945,12 @@ bool FSGXRTracker::FImpl::UpdateSGIndexJointData( static constexpr int32 IndexTipJointIndex = 3; const FTransform& IndexMetacarpalTransform{ - GetOpenXRMeshMetacarpalBoneRefTransform(DeviceHand, EHandKeypoint::IndexMetacarpal) + GetOpenXRMeshBoneRefTransform(DeviceHand, EHandKeypoint::IndexMetacarpal) }; - FVector IndexMetacarpalLocation{IndexMetacarpalTransform.GetLocation()}; - FQuat IndexMetacarpalRotation{IndexMetacarpalTransform.GetRotation()}; + // FVector IndexMetacarpalLocation{IndexMetacarpalTransform.GetLocation()}; + // FQuat IndexMetacarpalRotation{IndexMetacarpalTransform.GetRotation()}; + FVector IndexMetacarpalLocation{3.759f, 2.089f, -0.932f}; + FQuat IndexMetacarpalRotation{0.000000000f, 0.000000000f, 0.000000000f, 0.999999829f}; const FVector& IndexProximalLocation{JointLocations[IndexFingerIndex][IndexProximalJointIndex]}; const FVector& IndexIntermediateLocation{JointLocations[IndexFingerIndex][IndexIntermediateJointIndex]}; @@ -1972,10 +2026,12 @@ bool FSGXRTracker::FImpl::UpdateSGMiddleJointData( static constexpr int32 MiddleTipJointMiddle = 3; const FTransform& MiddleMetacarpalTransform{ - GetOpenXRMeshMetacarpalBoneRefTransform(DeviceHand, EHandKeypoint::MiddleMetacarpal) + GetOpenXRMeshBoneRefTransform(DeviceHand, EHandKeypoint::MiddleMetacarpal) }; - FVector MiddleMetacarpalLocation{MiddleMetacarpalTransform.GetLocation()}; - FQuat MiddleMetacarpalRotation{MiddleMetacarpalTransform.GetRotation()}; + // FVector MiddleMetacarpalLocation{MiddleMetacarpalTransform.GetLocation()}; + // FQuat MiddleMetacarpalRotation{MiddleMetacarpalTransform.GetRotation()}; + FVector MiddleMetacarpalLocation{3.534f, 0.405f, -0.767f}; + FQuat MiddleMetacarpalRotation{0.000000000f, 0.000000000f, 0.000000000f, 0.999999829f}; const FVector& MiddleProximalLocation{JointLocations[MiddleFingerMiddle][MiddleProximalJointMiddle]}; const FVector& MiddleIntermediateLocation{JointLocations[MiddleFingerMiddle][MiddleIntermediateJointMiddle]}; @@ -2051,10 +2107,12 @@ bool FSGXRTracker::FImpl::UpdateSGRingJointData( static constexpr int32 RingTipJointRing = 3; const FTransform& RingMetacarpalTransform{ - GetOpenXRMeshMetacarpalBoneRefTransform(DeviceHand, EHandKeypoint::RingMetacarpal) + GetOpenXRMeshBoneRefTransform(DeviceHand, EHandKeypoint::RingMetacarpal) }; - FVector RingMetacarpalLocation{RingMetacarpalTransform.GetLocation()}; - FQuat RingMetacarpalRotation{RingMetacarpalTransform.GetRotation()}; + // FVector RingMetacarpalLocation{RingMetacarpalTransform.GetLocation()}; + // FQuat RingMetacarpalRotation{RingMetacarpalTransform.GetRotation()}; + FVector RingMetacarpalLocation{3.612f, -1.557f, -0.625f}; + FQuat RingMetacarpalRotation{0.000000000f, 0.000000000f, 0.000000000f, 0.999999829f}; const FVector& RingProximalLocation{JointLocations[RingFingerRing][RingProximalJointRing]}; const FVector& RingIntermediateLocation{JointLocations[RingFingerRing][RingIntermediateJointRing]}; @@ -2130,10 +2188,12 @@ bool FSGXRTracker::FImpl::UpdateSGLittleJointData( static constexpr int32 LittleTipJointLittle = 3; const FTransform& LittleMetacarpalTransform{ - GetOpenXRMeshMetacarpalBoneRefTransform(DeviceHand, EHandKeypoint::LittleMetacarpal) + GetOpenXRMeshBoneRefTransform(DeviceHand, EHandKeypoint::LittleMetacarpal) }; - FVector LittleMetacarpalLocation{LittleMetacarpalTransform.GetLocation()}; - FQuat LittleMetacarpalRotation{LittleMetacarpalTransform.GetRotation()}; + // FVector LittleMetacarpalLocation{LittleMetacarpalTransform.GetLocation()}; + // FQuat LittleMetacarpalRotation{LittleMetacarpalTransform.GetRotation()}; + FVector LittleMetacarpalLocation{3.539f, -2.389f, -0.978f}; + FQuat LittleMetacarpalRotation{0.207036002f, 0.018311810f, -0.140342834f, 0.968041580f}; const FVector& LittleProximalLocation{JointLocations[LittleFingerLittle][LittleProximalJointLittle]}; const FVector& LittleIntermediateLocation{JointLocations[LittleFingerLittle][LittleIntermediateJointLittle]};