Merge pull request #15 from qualisys/fix_skeleton_mapping

qualisys · Dec 11, 2020 · d1a5950 · d1a5950
2 parents b87769e + 2305d73
commit d1a5950
Show file tree

Hide file tree

Showing 18 changed files with 16 additions and 14 deletions.
diff --git a/Example Project/Content/BackFlip.umap b/Example Project/Content/BackFlip.umap
diff --git a/Example Project/Content/BackFlip_BuiltData.uasset b/Example Project/Content/BackFlip_BuiltData.uasset
diff --git a/Example Project/Content/QAvatar/Body_D_red.uasset b/Example Project/Content/QAvatar/Body_D_red.uasset
diff --git a/Example Project/Content/QAvatar/Body_R.uasset b/Example Project/Content/QAvatar/Body_R.uasset
diff --git a/Example Project/Content/QAvatar/Mask_D.uasset b/Example Project/Content/QAvatar/Mask_D.uasset
diff --git a/Example Project/Content/QAvatar/QAFace.uasset b/Example Project/Content/QAvatar/QAFace.uasset
diff --git a/Example Project/Content/QAvatar/QAMask.uasset b/Example Project/Content/QAvatar/QAMask.uasset
diff --git a/Example Project/Content/QAvatar/QASuit_B.uasset b/Example Project/Content/QAvatar/QASuit_B.uasset
diff --git a/Example Project/Content/QAvatar/QAvatar.uasset b/Example Project/Content/QAvatar/QAvatar.uasset
diff --git a/Example Project/Content/QAvatar/QAvatar_PhysicsAsset.uasset b/Example Project/Content/QAvatar/QAvatar_PhysicsAsset.uasset
diff --git a/Example Project/Content/QAvatar/QAvatar_Skeleton.uasset b/Example Project/Content/QAvatar/QAvatar_Skeleton.uasset
diff --git a/Example Project/Content/QAvatar/QAvatar_Skeleton_AnimBlueprintNoRetarget.uasset b/Example Project/Content/QAvatar/QAvatar_Skeleton_AnimBlueprintNoRetarget.uasset
diff --git a/Example Project/Content/QAvatar/QAvatar_Skeleton_AnimBlueprintRetarget.uasset b/Example Project/Content/QAvatar/QAvatar_Skeleton_AnimBlueprintRetarget.uasset
diff --git a/Example Project/Content/QAvatar/face_D.uasset b/Example Project/Content/QAvatar/face_D.uasset
diff --git a/Qualisys/QTMConnect/ThirdParty/RTClientSDK/Lib/Win64/RTClientSDK.lib b/Qualisys/QTMConnect/ThirdParty/RTClientSDK/Lib/Win64/RTClientSDK.lib
diff --git a/Qualisys/QTMConnectLiveLink/Source/QTMConnectLiveLink/Private/QTMConnectLiveLinkSource.cpp b/Qualisys/QTMConnectLiveLink/Source/QTMConnectLiveLink/Private/QTMConnectLiveLinkSource.cpp
@@ -466,22 +466,20 @@ uint32 FQTMConnectLiveLinkSource::Run()
 
                         CRTPacket::SSkeletonSegment segment;
                         packet->GetSkeletonSegment(skeletonIndex, segmentIndex, segment);
+
+                        auto segmentRotation = FQuat(-segment.rotationX, segment.rotationY, -segment.rotationZ, segment.rotationW) * FQuat(-settings.rotationX, settings.rotationY, -settings.rotationZ, settings.rotationW).Inverse();
 
-                        auto segmentRotation = FQuat(0, 0, 0, 1);
-                        auto segmentLocation = FVector(0, 0, 0);
+                        while (settings.parentIndex != -1)
+                        {
+                            mRTProtocol->GetSkeletonSegment(skeletonIndex, settings.parentIndex, &settings);
 
+                            auto ancestorRotation = FQuat(-settings.rotationX, settings.rotationY, -settings.rotationZ, settings.rotationW);
 
-                        if (settings.parentIndex == -1)
-                        {
-                            segmentRotation = FQuat(segment.rotationX, -segment.rotationY, segment.rotationZ, -segment.rotationW);
-                            segmentLocation = FVector(segment.positionX, -segment.positionY, segment.positionZ) * positionScalingFactor;
-                        }
-                        else
-                        {
-                            segmentRotation = FQuat(-segment.rotationX, segment.rotationY, -segment.rotationZ, segment.rotationW);
-                            segmentLocation = FVector(segment.positionX, -segment.positionY, segment.positionZ) * positionScalingFactor;
+                            segmentRotation = ancestorRotation * segmentRotation * ancestorRotation.Inverse();
                         }
 
+                        const auto segmentLocation = FVector(segment.positionX, -segment.positionY, segment.positionZ) * positionScalingFactor;
+
                         const auto segmentScale = FVector(1.0, 1.0, 1.0);
 
                         transforms[segmentIndex] = FTransform(segmentRotation, segmentLocation, segmentScale);

diff --git a/...ys/QTMConnectLiveLink/Source/QTMConnectLiveLink/Private/QualisysLiveLinkRetargetAsset.cpp b/...ys/QTMConnectLiveLink/Source/QTMConnectLiveLink/Private/QualisysLiveLinkRetargetAsset.cpp
@@ -192,7 +192,11 @@ void UQualisysLiveLinkRetargetAsset::BuildPoseFromAnimationData(float DeltaTime,
     for (int32 i = 0; i < TargetBoneNames.Num(); ++i)
     {
         const auto& BoneName = TargetBoneNames[i];
-        const auto& BoneTransform = InFrameData->Transforms[i];
+        auto BoneTransform = InFrameData->Transforms[i];
+        const auto& location = BoneTransform.GetLocation();
+        const auto& rotation = BoneTransform.GetRotation();
+        BoneTransform.SetLocation(FVector(-location.X, -location.Y, location.Z));
+        BoneTransform.SetRotation(FQuat(-rotation.X, -rotation.Y, rotation.Z, rotation.W));
 
         const auto BoneIndex = GetCompactPoseBoneIndex(BoneName, OutPose);
 
@@ -229,7 +233,7 @@ void UQualisysLiveLinkRetargetAsset::BuildPoseFromAnimationData(float DeltaTime,
                     const auto ChildActualDir = (ChildRefPose.GetTranslation() - WorldRefPose.GetTranslation()).GetSafeNormal();
                     const auto ChildTargetDir = GetTargetBoneDir(SourceBoneNames[i]);
 
-                    LocalPoseCorrections[i] = FQuat::MakeFromEuler(FVector(0, 0, 0));
+                    LocalPoseCorrections[i] = FQuat::FindBetween(WorldToLocal.RotateVector(ChildActualDir), WorldToLocal.RotateVector(ChildTargetDir));
                 }
             }
         }
@@ -242,7 +246,7 @@ void UQualisysLiveLinkRetargetAsset::BuildPoseFromAnimationData(float DeltaTime,
             const auto OriginalPose = GetRefPose(OutPose, BoneIndex, false);
             const auto CorrectedPose = GetRefPose(OutPose, BoneIndex, true);
 
-            LocalPoseCorrections[i] = FQuat::MakeFromEuler(FVector(0, 0, 0));
+            LocalPoseCorrections[i] = (CorrectedPose.GetRotation().Inverse() * OriginalPose.GetRotation());
         }
 
         auto Pose = FTransform::Identity;

diff --git a/Qualisys/QTMConnectLiveLink/ThirdParty/RTClientSDK/Lib/Win64/RTClientSDK.lib b/Qualisys/QTMConnectLiveLink/ThirdParty/RTClientSDK/Lib/Win64/RTClientSDK.lib