UnrealEngine/Plugin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

somewhat unfinished refactored sg bone calculations

Browse Source
This commit is contained in:
Mamadou Babaei
2024-08-16 18:02:04 +02:00
parent 332b930e45
commit 23dff3d8a9
1 changed files with 149 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/SenseGloveTracking/Private/SGTracking/SGXRTracker.cpp
+149 -89
View File
@@ -206,7 +206,7 @@ struct FSGXRTracker::FImpl
static TArray<float> GetFingerLengths(
bool bRight,
EHandKeypoint Joint1st, EHandKeypoint Joint2nd, EHandKeypoint Joint3rd,
EHandKeypoint Joint2nd, EHandKeypoint Joint3rd,
float DistalPhalanxLength);
static TArray<TArray<float>> 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<float> 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<float>(FVector::Dist(Joint1stLocation, Joint2ndLocation));
const float PhalanxLength2nd = static_cast<float>(FVector::Dist(Joint2ndLocation, Joint3rdLocation));
const float PhalanxLength1st = static_cast<float>(Joint2ndLocation.Size());
const float PhalanxLength2nd = static_cast<float>(Joint3rdLocation.Size());
const TArray<float> Lengths{PhalanxLength1st, PhalanxLength2nd, DistalPhalanxLength};
return Lengths;
@@ -657,31 +655,26 @@ TArray<TArray<float>> FSGXRTracker::FImpl::GetFingerLengths(const bool bRight)
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
@@ -695,47 +688,64 @@ TArray<FVector> 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<FVector> StartPositions{
ThumbStartPosition,
IndexStartPosition,
@@ -749,44 +759,83 @@ TArray<FVector> FSGXRTracker::FImpl::GetFingerStartPositions(const bool bRight)
TArray<FQuat> 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<FQuat> 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]};