Fix : 필터 시스템 업데이트

Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml

@@ -42,6 +42,14 @@
         </ui:Foldout>
     </ui:VisualElement>
 
+    <!-- Skeleton Frame Filter -->
+    <ui:VisualElement class="section">
+        <ui:Foldout text="Skeleton Frame Filter" value="true" class="section-foldout">
+            <uie:PropertyField binding-path="EnableSkeletonFrameFilter" label="Enable Filter (엄격)"/>
+            <ui:HelpBox message-type="Info" text="ON: 본 하나라도 트래킹 실패/손상되면 그 프레임 전체를 폐기 → 떨림은 줄지만 라이브에서 마커 가림 시 액터가 얼어붙을 수 있음.&#10;OFF(권장·라이브): 정상 본만 갱신, 미트래킹·손상 본은 직전 포즈 유지 → 모션이 끊기지 않음."/>
+        </ui:Foldout>
+    </ui:VisualElement>
+
     <!-- NatNet Version -->
     <ui:VisualElement class="section">
         <ui:Foldout text="NatNet Version" value="false" class="section-foldout">

Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs

@@ -373,6 +373,12 @@ public class OptitrackStreamingClient : MonoBehaviour
     [Tooltip("모든 스켈레톤 본과 리지드바디의 좌우를 거울 반전합니다.")]
     public bool MirrorMode = false;
 
+    [Header("Skeleton Frame Filter")]
+    [Tooltip("스켈레톤 프레임 무결성 필터입니다.\n" +
+        "ON: 본 하나라도 트래킹 실패·손상되면 그 프레임 전체를 폐기해 떨림/순간 깨짐을 막습니다. 대신 라이브에서 마커 가림이 잦으면 액터가 얼어붙을 수 있습니다.\n" +
+        "OFF(권장·라이브): 정상 본만 갱신하고 미트래킹·손상 본은 직전 포즈를 유지해 모션이 끊기지 않습니다. (NaN/0-쿼터니언 본은 모드와 무관하게 개별로 건너뜁니다.)")]
+    public bool EnableSkeletonFrameFilter = false;
+
 
     #region Private fields
     //private UInt16 ServerCommandPort = NatNetConstants.DefaultCommandPort;
@@ -2074,46 +2080,57 @@ public class OptitrackStreamingClient : MonoBehaviour
                     continue;
                 }
 
-                // Motive can occasionally emit an empty or partial skeleton payload. Do not let
+                // === 스켈레톤 프레임 필터 (EnableSkeletonFrameFilter) ===
-                // one bad bone overwrite the previous valid pose: stage and validate first.
+                // Motive가 빈/부분 스켈레톤 페이로드를 내보낼 수 있다.
-                if (skelRbCount != skelDef.Bones.Count)
+                //   ON : 본 개수가 다르면 프레임 전체 폐기 (직전 포즈 유지 → 떨림/부분프레임 방지)
+                //   OFF: 들어온 본 개수만큼 그대로 처리 (모션 끊김 방지)
+                if (EnableSkeletonFrameFilter && skelRbCount != skelDef.Bones.Count)
                 {
                     continue;
                 }
 
                 sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, skelRbCount);
-                bool isValidSkeletonFrame = true;
                 for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                 {
                     result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out stagedBones[boneIdx] );
                     NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetRigidBody failed." );
+                }
 
-                    sRigidBodyData boneData = stagedBones[boneIdx];
+                // ON 일 때만: 본 하나라도 트래킹 실패/손상이면 프레임 전체 폐기
-                    // In the context of frame data (unlike in the definition data), this ID value is a
+                if (EnableSkeletonFrameFilter)
-                    // packed composite of both the asset/entity (skeleton) ID and member (bone) ID.
+                {
-                    Int32 boneSkelId, boneId;
+                    bool isValidSkeletonFrame = true;
-                    NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
+                    for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
-
-                    if (boneSkelId != skeletonId ||
-                        !skelDef.BoneIdToParentIdMap.ContainsKey(boneId) ||
-                        !IsValidSkeletonBoneData(boneData))
                     {
-                        isValidSkeletonFrame = false;
+                        sRigidBodyData boneData = stagedBones[boneIdx];
-                        break;
+                        // In the context of frame data (unlike in the definition data), this ID value is a
+                        // packed composite of both the asset/entity (skeleton) ID and member (bone) ID.
+                        Int32 boneSkelId, boneId;
+                        NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
+
+                        if (boneSkelId != skeletonId ||
+                            !skelDef.BoneIdToParentIdMap.ContainsKey(boneId) ||
+                            !IsSkeletonBoneTracked(boneData) ||
+                            !IsBoneDataUsable(boneData))
+                        {
+                            isValidSkeletonFrame = false;
+                            break;
+                        }
+                    }
+
+                    if (!isValidSkeletonFrame)
+                    {
+                        continue;
                     }
                 }
 
-                if (!isValidSkeletonFrame)
+                // === 글로벌 트랜스폼 커밋 ===
-                {
+                // 필터 OFF 에서도 손상(NaN·0쿼터니언)·미매핑 본은 개별로 건너뛰어 직전 포즈를 보존한다.
-                    continue;
-                }
-
-                // Commit global transforms only after the whole payload has passed validation.
                 for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                 {
                     sRigidBodyData boneData = stagedBones[boneIdx];
-                    Int32 boneSkelId, boneId;
+                    if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
-                    NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
+                        continue;
 
                     // TODO: Could pre-populate this map when the definitions are retrieved.
                     // Should never allocate after the first frame, at least.
@@ -2137,8 +2154,8 @@ public class OptitrackStreamingClient : MonoBehaviour
                 for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                 {
                     sRigidBodyData boneData = stagedBones[boneIdx];
-                    Int32 boneSkelId, boneId;
+                    if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
-                    NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
+                        continue;
 
                     Vector3 bonePos = skelState.BonePoses[boneId].Position;
                     Quaternion boneOri = skelState.BonePoses[boneId].Orientation;
@@ -2146,10 +2163,10 @@ public class OptitrackStreamingClient : MonoBehaviour
                     Quaternion parentBoneOri = new Quaternion(0,0,0,1);
 
                     Int32 pId = skelDef.BoneIdToParentIdMap[boneId];
-                    if (pId != 0)
+                    if (pId != 0 && skelState.BonePoses.TryGetValue(pId, out OptitrackPose parentPose))
                     {
-                        parentBonePos = skelState.BonePoses[pId].Position;
+                        parentBonePos = parentPose.Position;
-                        parentBoneOri = skelState.BonePoses[pId].Orientation;
+                        parentBoneOri = parentPose.Orientation;
                     }
                     skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
                     skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
@@ -2746,11 +2763,18 @@ public class OptitrackStreamingClient : MonoBehaviour
         return scratch;
     }
 
-    private static bool IsValidSkeletonBoneData(sRigidBodyData boneData)
+    /// <summary>Motive "이 프레임 트래킹됨" 비트(0x01). 엄격 필터(ON)에서만 프레임 폐기 조건으로 쓰인다.</summary>
+    private static bool IsSkeletonBoneTracked(sRigidBodyData boneData)
     {
-        if ((boneData.Params & 0x01) == 0)
+        return (boneData.Params & 0x01) != 0;
-            return false;
+    }
 
+    /// <summary>
+    /// 본 데이터가 포즈로 커밋해도 안전한지 검사한다(좌표 유한 + 쿼터니언 정상).
+    /// 트래킹 비트와 무관 — 필터 OFF에서도 손상 본이 직전 포즈를 덮어쓰지 못하게 막는 안전장치.
+    /// </summary>
+    private static bool IsBoneDataUsable(sRigidBodyData boneData)
+    {
         float quaternionMagnitudeSquared =
             boneData.QX * boneData.QX +
             boneData.QY * boneData.QY +
@@ -2767,6 +2791,20 @@ public class OptitrackStreamingClient : MonoBehaviour
             quaternionMagnitudeSquared > 0.000001f;
     }
 
+    /// <summary>
+    /// 프레임 본 데이터가 이 스켈레톤에 커밋 가능한지 검사하고 boneId를 출력한다.
+    /// 스켈레톤 ID 일치 + 정의에 매핑된 본 + 데이터 정상(NaN/0-쿼터니언 아님)일 때만 true.
+    /// (필터 ON/OFF 공통으로 커밋 단계에서 사용 — OFF에서 손상·미매핑 본 개별 스킵, 2차 패스 KeyNotFound 방지)
+    /// </summary>
+    private static bool TryGetCommittableBoneId(sRigidBodyData boneData, Int32 skeletonId, OptitrackSkeletonDefinition skelDef, out int boneId)
+    {
+        Int32 boneSkelId;
+        NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
+        return boneSkelId == skeletonId
+            && skelDef.BoneIdToParentIdMap.ContainsKey(boneId)
+            && IsBoneDataUsable(boneData);
+    }
+
     private static bool IsFinite(float value)
     {
         return !float.IsNaN(value) && !float.IsInfinity(value);