diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackSkeletonAnimator_Mingle.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackSkeletonAnimator_Mingle.cs index 3a7cba5bf..c52694824 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackSkeletonAnimator_Mingle.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackSkeletonAnimator_Mingle.cs @@ -1,5 +1,7 @@ using System; using System.Collections.Generic; +using System.IO; +using System.Text; using UnityEngine; using System.Collections; @@ -81,6 +83,83 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour SetFilterStrength((FilterStrength)next); } + // ── 패킷 녹화 ────────────────────────────────────────────────────────── + [Header("패킷 녹화 (지터 분석용)")] + [Tooltip("활성화하면 raw 스냅샷 데이터를 CSV로 기록합니다")] + public bool enableRecording = false; + + private StreamWriter m_csvWriter; + private float m_recordStartTime; + private bool m_isRecording; + private List<(string name, int id)> m_recordBoneList = new List<(string, int)>(); + + public void StartRecording() + { + if (m_isRecording) return; + if (m_skeletonDef == null) { Debug.LogWarning("[OptiTrack] 스켈레톤 미연결 — 녹화 불가"); return; } + + // 전체 본 목록 구축 + m_recordBoneList.Clear(); + foreach (var bone in m_skeletonDef.Bones) + { + string optiName = bone.Name.Contains("_") ? bone.Name[(bone.Name.IndexOf('_') + 1)..] : bone.Name; + m_recordBoneList.Add((optiName, bone.Id)); + } + + string timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss"); + string path = Path.Combine(Application.persistentDataPath, $"optitrack_raw_{timestamp}.csv"); + + m_csvWriter = new StreamWriter(path, false, Encoding.UTF8); + + // 헤더: Time, NatNetDt, 본별 px,py,pz,rx,ry,rz,rw + var header = new StringBuilder("Time,NatNetDt"); + foreach (var (name, _) in m_recordBoneList) + { + header.Append($",{name}_px,{name}_py,{name}_pz"); + header.Append($",{name}_rx,{name}_ry,{name}_rz,{name}_rw"); + } + m_csvWriter.WriteLine(header); + + m_recordStartTime = Time.realtimeSinceStartup; + m_isRecording = true; + Debug.Log($"[OptiTrack] 녹화 시작 ({m_recordBoneList.Count}개 본): {path}"); + } + + public void StopRecording() + { + if (!m_isRecording) return; + m_isRecording = false; + + m_csvWriter?.Flush(); + m_csvWriter?.Close(); + m_csvWriter = null; + Debug.Log("[OptiTrack] 녹화 중지 — 파일 저장 완료"); + } + + private void RecordFrame() + { + if (!m_isRecording || m_csvWriter == null) return; + + float elapsed = Time.realtimeSinceStartup - m_recordStartTime; + var line = new StringBuilder(); + line.Append($"{elapsed:F6},{m_natNetDt:F6}"); + + foreach (var (_, boneId) in m_recordBoneList) + { + Vector3 pos = m_snapshotPositions.TryGetValue(boneId, out Vector3 p) ? p : Vector3.zero; + Quaternion rot = m_snapshotOrientations.TryGetValue(boneId, out Quaternion q) ? q : Quaternion.identity; + line.Append($",{pos.x:F6},{pos.y:F6},{pos.z:F6}"); + line.Append($",{rot.x:F6},{rot.y:F6},{rot.z:F6},{rot.w:F6}"); + } + + m_csvWriter.WriteLine(line); + } + + void OnDestroy() + { + StopRecording(); + } + private OptitrackSkeletonDefinition m_skeletonDef; private string previousSkeletonName; @@ -89,6 +168,9 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour private const float k_SkeletonCheckInterval = 0.1f; + private Coroutine m_checkCoroutine; + private bool m_initialized = false; + // 본 ID → 매핑 인덱스 (빠른 룩업) private Dictionary m_boneIdToMappingIndex = new Dictionary(); // 본 이름 → Transform 캐시 (전체 하이어라키) @@ -219,11 +301,6 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour BuildTransformCache(); InitializeStreamingClient(); - if (StreamingClient != null) - { - StreamingClient.RegisterSkeleton(this, this.SkeletonAssetName); - } - // SO에서 본 매핑 + T-포즈 로드 if (RestPoseAsset != null && RestPoseAsset.boneMappings.Count > 0) { @@ -260,7 +337,46 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour } previousSkeletonName = SkeletonAssetName; - StartCoroutine(CheckSkeletonConnectionPeriodically()); + m_initialized = true; + + // 등록 및 코루틴은 OnEnable()에서 처리 + RegisterAndStartChecking(); + } + + void OnEnable() + { + // Start()가 아직 실행되지 않았으면 무시 (Start()에서 RegisterAndStartChecking 호출됨) + if (!m_initialized) return; + + // 클라이언트 참조가 끊어졌으면 재탐색 + if (StreamingClient == null) + InitializeStreamingClient(); + + RegisterAndStartChecking(); + } + + void OnDisable() + { + isSkeletonFound = false; + + if (m_checkCoroutine != null) + { + StopCoroutine(m_checkCoroutine); + m_checkCoroutine = null; + } + } + + private void RegisterAndStartChecking() + { + if (StreamingClient != null) + { + StreamingClient.RegisterSkeleton(this, this.SkeletonAssetName); + previousSkeletonName = SkeletonAssetName; + } + + // 코루틴이 이미 돌고 있지 않으면 시작 + if (m_checkCoroutine == null) + m_checkCoroutine = StartCoroutine(CheckSkeletonConnectionPeriodically()); } private bool m_lastMirrorMode = false; @@ -297,6 +413,10 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour if (m_skeletonDef == null) return; + // enableRecording 토글 감지 + if (enableRecording && !m_isRecording) StartRecording(); + else if (!enableRecording && m_isRecording) StopRecording(); + // 락 보호 하에 스냅샷 복사 + NatNet 하드웨어 타임스탬프 수신 OptitrackHiResTimer.Timestamp frameTs; if (!StreamingClient.FillBoneSnapshot(m_skeletonDef.Id, m_snapshotPositions, m_snapshotOrientations, out frameTs)) @@ -312,6 +432,9 @@ public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour m_lastFrameTimestamp = frameTs; m_hasLastFrameTimestamp = true; + // 패킷 녹화 (필터 적용 전 raw 데이터) + RecordFrame(); + // ── Pass 1: Raw 데이터 적용 → IK 포인트 월드 위치 캡처 ────────────────── // 필터가 활성화되어 있을 때만 two-pass, 비활성이면 single-pass if (enableBoneFilter) diff --git a/Assets/Scripts/KindRetargeting/CustomRetargetingScript.cs b/Assets/Scripts/KindRetargeting/CustomRetargetingScript.cs index af4ede1a7..52d8f3bd8 100644 --- a/Assets/Scripts/KindRetargeting/CustomRetargetingScript.cs +++ b/Assets/Scripts/KindRetargeting/CustomRetargetingScript.cs @@ -1200,13 +1200,8 @@ namespace KindRetargeting ikSolver.rightLeg.sourceLower = GetSourceBoneTransform(HumanBodyBones.RightLowerLeg); ikSolver.rightLeg.sourceEnd = GetSourceBoneTransform(HumanBodyBones.RightFoot); - ikSolver.leftArm.sourceUpper = GetSourceBoneTransform(HumanBodyBones.LeftUpperArm); - ikSolver.leftArm.sourceLower = GetSourceBoneTransform(HumanBodyBones.LeftLowerArm); - ikSolver.leftArm.sourceEnd = GetSourceBoneTransform(HumanBodyBones.LeftHand); - - ikSolver.rightArm.sourceUpper = GetSourceBoneTransform(HumanBodyBones.RightUpperArm); - ikSolver.rightArm.sourceLower = GetSourceBoneTransform(HumanBodyBones.RightLowerArm); - ikSolver.rightArm.sourceEnd = GetSourceBoneTransform(HumanBodyBones.RightHand); + // 팔은 소스 참조 없이 bendGoal 기반 cosine law 사용 + // (180° 특이점/역관절 이슈가 없고, bendGoal 힌트 방향이 더 정확) } } diff --git a/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs b/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs index 567bb079a..f3b240ce3 100644 --- a/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs +++ b/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs @@ -98,44 +98,53 @@ namespace KindRetargeting private void SolveLimb(LimbIK limb) { - if (!limb.enabled || limb.target == null || limb.positionWeight < 0.001f) return; + if (!limb.enabled || limb.target == null) return; if (limb.upper == null || limb.lower == null || limb.end == null) return; + float weight = limb.positionWeight; + if (weight < 0.0001f) return; // 완전히 0일 때만 스킵 + + // FK 회전 저장 (IK 결과와 블렌딩용) + Quaternion fkUpperRot = limb.upper.rotation; + Quaternion fkLowerRot = limb.lower.rotation; + Quaternion fkEndRot = limb.end.rotation; + float upperLen = Vector3.Distance(limb.upper.position, limb.lower.position); float lowerLen = Vector3.Distance(limb.lower.position, limb.end.position); - Vector3 targetPos = Vector3.Lerp(limb.end.position, limb.target.position, limb.positionWeight); + Vector3 targetPos = limb.target.position; // --- 무릎 위치 결정 --- Vector3 kneePos; if (limb.sourceUpper != null && limb.sourceLower != null && limb.sourceEnd != null) { - // 소스 무릎 위치 기반: 소스의 무릎 위치를 타겟 비율로 스케일 kneePos = ComputeKneePosFromSource(limb, upperLen, lowerLen, targetPos); } else { - // 소스 참조 없음: 기존 bendGoal 기반 fallback (팔 등) kneePos = ComputeKneePosFromBendGoal(limb, upperLen, lowerLen, targetPos); } - // --- 본 회전 적용 --- + // --- IK 회전 계산 --- Vector3 currentUpperDir = (limb.lower.position - limb.upper.position).normalized; Vector3 desiredUpperDir = (kneePos - limb.upper.position).normalized; - limb.upper.rotation = Quaternion.FromToRotation(currentUpperDir, desiredUpperDir) * limb.upper.rotation; + Quaternion ikUpperRot = Quaternion.FromToRotation(currentUpperDir, desiredUpperDir) * limb.upper.rotation; + // upper 적용 후 lower 계산 (자식 위치가 바뀌므로 임시 적용) + limb.upper.rotation = ikUpperRot; Vector3 currentLowerDir = (limb.end.position - limb.lower.position).normalized; Vector3 desiredLowerDir = (targetPos - limb.lower.position).normalized; - limb.lower.rotation = Quaternion.FromToRotation(currentLowerDir, desiredLowerDir) * limb.lower.rotation; + Quaternion ikLowerRot = Quaternion.FromToRotation(currentLowerDir, desiredLowerDir) * limb.lower.rotation; - if (limb.rotationWeight > 0.001f) + // --- FK/IK 블렌딩: weight로 부드럽게 전환 --- + limb.upper.rotation = Quaternion.Slerp(fkUpperRot, ikUpperRot, weight); + limb.lower.rotation = Quaternion.Slerp(fkLowerRot, ikLowerRot, weight); + + if (limb.rotationWeight > 0.0001f) { - limb.end.rotation = Quaternion.Slerp( - limb.end.rotation, - limb.target.rotation, - limb.rotationWeight - ); + Quaternion ikEndRot = limb.target.rotation; + limb.end.rotation = Quaternion.Slerp(fkEndRot, ikEndRot, limb.rotationWeight); } } @@ -179,10 +188,15 @@ namespace KindRetargeting return limb.lower.position; Vector3 targetHipToFootDir = targetHipToFoot / targetHipToFootMag; - // 타겟 무릎 위치: 타겟 hip에서 투영 + 수직 성분 적용 + // 소스 프레임 → 타겟 프레임으로 수직 성분 회전 + // (소스와 타겟의 사지 방향이 다를 때 팔꿈치/무릎 오프셋 방향 보정) + Quaternion frameRotation = Quaternion.FromToRotation(sourceHipToFootDir, targetHipToFootDir); + Vector3 rotatedRejection = frameRotation * scaledRejection; + + // 타겟 관절 위치: 타겟 upper에서 투영 + 회전된 수직 성분 적용 Vector3 kneePos = limb.upper.position + targetHipToFootDir * scaledProjection - + scaledRejection; + + rotatedRejection; return kneePos; } @@ -199,29 +213,55 @@ namespace KindRetargeting if (targetDist < 0.001f) return limb.lower.position; Vector3 toTargetDir = toTarget / targetDist; - Vector3 bendNormal = limb.upper.rotation * limb.localBendNormal; - + // bendGoal의 수직 성분(rejection)으로 팔꿈치/무릎 방향 직접 결정 + Vector3 bendDir; if (limb.bendGoal != null && limb.bendGoalWeight > 0.001f) { - Vector3 goalNormal = Vector3.Cross( - limb.bendGoal.position - limb.upper.position, - targetPos - limb.upper.position - ); - if (goalNormal.sqrMagnitude > 0.01f) + Vector3 toBendGoal = limb.bendGoal.position - limb.upper.position; + Vector3 rejection = toBendGoal - Vector3.Dot(toBendGoal, toTargetDir) * toTargetDir; + if (rejection.sqrMagnitude > 0.0001f) { - bendNormal = Vector3.Lerp(bendNormal, goalNormal.normalized, limb.bendGoalWeight); + // T-포즈 기반 기본 방향 + Vector3 defaultBendDir = GetDefaultBendDir(limb, toTargetDir); + bendDir = Vector3.Lerp(defaultBendDir, rejection.normalized, limb.bendGoalWeight); + } + else + { + bendDir = GetDefaultBendDir(limb, toTargetDir); } } - bendNormal.Normalize(); + else + { + bendDir = GetDefaultBendDir(limb, toTargetDir); + } + bendDir.Normalize(); + // 코사인 법칙으로 upper 각도 계산 float clampedDist = Mathf.Clamp(targetDist, Mathf.Abs(upperLen - lowerLen) + 0.001f, chainLength - 0.001f); - float cosUpper = (clampedDist * clampedDist + upperLen * upperLen - lowerLen * lowerLen) + float cosAngle = (clampedDist * clampedDist + upperLen * upperLen - lowerLen * lowerLen) / (2f * clampedDist * upperLen); - cosUpper = Mathf.Clamp(cosUpper, -1f, 1f); - float upperAngleDeg = Mathf.Acos(cosUpper) * Mathf.Rad2Deg; + cosAngle = Mathf.Clamp(cosAngle, -1f, 1f); + float angle = Mathf.Acos(cosAngle); - Vector3 kneeDir = Quaternion.AngleAxis(-upperAngleDeg, bendNormal) * toTargetDir; - return limb.upper.position + kneeDir * upperLen; + // 무릎/팔꿈치 위치: toTargetDir + bendDir 방향으로 angle만큼 오프셋 + // sin(angle) = 수직 성분, cos(angle) = 직선 성분 + Vector3 kneePos = limb.upper.position + + toTargetDir * (upperLen * Mathf.Cos(angle)) + + bendDir * (upperLen * Mathf.Sin(angle)); + + return kneePos; + } + + /// + /// T-포즈 기반 기본 굽힘 방향 (bendGoal이 없거나 불안정할 때) + /// + private Vector3 GetDefaultBendDir(LimbIK limb, Vector3 toTargetDir) + { + Vector3 bendNormal = limb.upper.rotation * limb.localBendNormal; + Vector3 bendDir = Vector3.Cross(bendNormal, toTargetDir); + if (bendDir.sqrMagnitude < 0.0001f) + bendDir = Vector3.Cross(Vector3.up, toTargetDir); + return bendDir.normalized; } public float CalculateAutoFloorHeight(float comfortRatio = 0.98f)