915 lines
36 KiB
C#
915 lines
36 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using System.Collections;
|
|
|
|
[System.Serializable]
|
|
public class OptiTrackBoneMapping
|
|
{
|
|
public string optiTrackBoneName; // OptiTrack 본 이름 (예: "Hip", "Ab")
|
|
public string fbxNodeName; // FBX 노드 이름 (예: "001_Hips", "001_Spine")
|
|
[HideInInspector]
|
|
public Transform cachedTransform; // 런타임 캐시
|
|
public bool applyPosition = true;
|
|
public bool applyRotation = true;
|
|
public bool isMapped = false; // 매핑 성공 여부
|
|
}
|
|
|
|
[DefaultExecutionOrder(-100)]
|
|
public class OptitrackSkeletonAnimator_Mingle : MonoBehaviour
|
|
{
|
|
[Header("OptiTrack 설정")]
|
|
[Tooltip("OptiTrackStreamingClient가 포함된 오브젝트")]
|
|
public OptitrackStreamingClient StreamingClient;
|
|
|
|
[Tooltip("Motive의 스켈레톤 에셋 이름")]
|
|
public string SkeletonAssetName = "Skeleton1";
|
|
|
|
// 런타임 본 매핑 — SO(RestPoseAsset)에서 로드됨. 직접 편집하지 말고 설정 창 사용.
|
|
[HideInInspector]
|
|
public List<OptiTrackBoneMapping> boneMappings = new List<OptiTrackBoneMapping>();
|
|
|
|
[Header("T-포즈 (기준 포즈)")]
|
|
[Tooltip("에디터에서 캡처한 T-포즈 데이터. 비어 있으면 런타임 CacheRestPose()를 사용합니다.")]
|
|
public OptitrackRestPoseData RestPoseAsset;
|
|
|
|
public enum FilterStrength { Off, Low, Medium, High, Custom }
|
|
|
|
[Header("본 1€ 필터 (속도 적응형 저역통과)")]
|
|
[HideInInspector]
|
|
public FilterStrength filterStrength = FilterStrength.Medium;
|
|
|
|
[Header("어깨 증폭")]
|
|
[Tooltip("어깨 회전을 증폭합니다. 1 = 원본, 2 = 2배. 하위 체인(상완)은 자동 역보정되어 손 위치가 유지됩니다.")]
|
|
[Range(0f, 10f)]
|
|
public float shoulderAmplify = 2f;
|
|
|
|
[HideInInspector] public float filterMinCutoff = 3.0f;
|
|
[HideInInspector] public float filterBeta = 1.5f;
|
|
[HideInInspector] public float filterMaxCutoff = 15.0f;
|
|
|
|
// 프리셋별 파라미터 (minCutoff, beta, maxCutoff)
|
|
private static readonly (float minCutoff, float beta, float maxCutoff)[] k_FilterPresets =
|
|
{
|
|
(0f, 0f, 0f), // Off (사용 안 함)
|
|
(5.0f, 2.0f, 25.0f), // Low
|
|
(3.0f, 1.5f, 15.0f), // Medium
|
|
(1.5f, 0.8f, 10.0f), // High
|
|
(0f, 0f, 0f), // Custom (프리셋 적용 안 함)
|
|
};
|
|
|
|
[HideInInspector] public bool enableBoneFilter = true;
|
|
|
|
/// <summary>
|
|
/// 런타임에서 필터 강도를 변경합니다. StreamDeck/핫키 등에서 호출.
|
|
/// </summary>
|
|
public void SetFilterStrength(FilterStrength strength)
|
|
{
|
|
filterStrength = strength;
|
|
if (strength != FilterStrength.Off && strength != FilterStrength.Custom)
|
|
{
|
|
var preset = k_FilterPresets[(int)strength];
|
|
filterMinCutoff = preset.minCutoff;
|
|
filterBeta = preset.beta;
|
|
filterMaxCutoff = preset.maxCutoff;
|
|
}
|
|
// 프리셋 전환 시 필터 상태 리셋 (이전 값과 불연속 방지)
|
|
m_filterStates.Clear();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 프리셋에서 다음 프리셋으로 순환. 버튼 하나로 Off→Low→Medium→High→Custom→Off...
|
|
/// </summary>
|
|
public void CycleFilterStrength()
|
|
{
|
|
int next = ((int)filterStrength + 1) % System.Enum.GetValues(typeof(FilterStrength)).Length;
|
|
SetFilterStrength((FilterStrength)next);
|
|
}
|
|
|
|
private OptitrackSkeletonDefinition m_skeletonDef;
|
|
private string previousSkeletonName;
|
|
|
|
[HideInInspector]
|
|
public bool isSkeletonFound = false;
|
|
|
|
private const float k_SkeletonCheckInterval = 0.1f;
|
|
|
|
private Coroutine m_checkCoroutine;
|
|
private bool m_initialized = false;
|
|
|
|
// 본 ID → 매핑 인덱스 (빠른 룩업)
|
|
private Dictionary<Int32, int> m_boneIdToMappingIndex = new Dictionary<Int32, int>();
|
|
// 본 이름 → Transform 캐시 (전체 하이어라키)
|
|
private Dictionary<string, Transform> m_allTransforms = new Dictionary<string, Transform>();
|
|
|
|
// FillBoneSnapshot 용 pre-allocated 스냅샷 버퍼 (락 내부에서 채워짐 → torn read 없음)
|
|
private Dictionary<Int32, Vector3> m_snapshotPositions = new Dictionary<Int32, Vector3>();
|
|
private Dictionary<Int32, Quaternion> m_snapshotOrientations = new Dictionary<Int32, Quaternion>();
|
|
|
|
// OptiTrack 본 이름 → Transform 빠른 캐시 (GetMappedTransform O(n) → O(1))
|
|
private Dictionary<string, Transform> m_optiNameTransformCache = new Dictionary<string, Transform>();
|
|
|
|
// 필터 적용 전 raw 월드 위치/회전 (IK 타겟용 — 접지력 보존)
|
|
private Dictionary<HumanBodyBones, Vector3> m_rawWorldPositions = new Dictionary<HumanBodyBones, Vector3>();
|
|
private Dictionary<HumanBodyBones, Quaternion> m_rawWorldRotations = new Dictionary<HumanBodyBones, Quaternion>();
|
|
|
|
// raw 위치를 캡처할 IK 포인트 본 목록
|
|
private static readonly HumanBodyBones[] k_IKPointBones = new HumanBodyBones[]
|
|
{
|
|
HumanBodyBones.LeftFoot,
|
|
HumanBodyBones.RightFoot,
|
|
HumanBodyBones.LeftHand,
|
|
HumanBodyBones.RightHand,
|
|
HumanBodyBones.LeftToes,
|
|
HumanBodyBones.RightToes,
|
|
};
|
|
|
|
|
|
// 스파인/넥 체인 Transform 캐시 (GetSpineChainTransforms 매 호출 List 할당 방지)
|
|
private List<Transform> m_spineChainCache = new List<Transform>();
|
|
private List<Transform> m_neckChainCache = new List<Transform>();
|
|
|
|
// 힙 본 ID 캐시 (-1 = 미발견)
|
|
private int m_hipBoneId = -1;
|
|
|
|
// NatNet 하드웨어 타임스탬프 기반 실제 프레임 간격 (렌더 프레임과 독립)
|
|
private float m_natNetDt = 1f / 120f;
|
|
private OptitrackHiResTimer.Timestamp m_lastFrameTimestamp;
|
|
private bool m_hasLastFrameTimestamp = false;
|
|
|
|
// 1€ 필터 상태 (본 ID → 이전 프레임 필터 상태)
|
|
private struct BoneFilterState
|
|
{
|
|
public Quaternion prevOri;
|
|
public float dOriMag; // 필터된 각속도 크기 (rad/s)
|
|
public Vector3 prevPos;
|
|
public float dPosMag; // 필터된 선속도 크기 (m/s)
|
|
public bool initialized;
|
|
}
|
|
private Dictionary<Int32, BoneFilterState> m_filterStates = new Dictionary<Int32, BoneFilterState>();
|
|
|
|
// OptiTrack 본 이름 → FBX 노드 접미사 기본 매핑
|
|
public static readonly Dictionary<string, string> DefaultOptiToFbxSuffix = new Dictionary<string, string>
|
|
{
|
|
// 몸통 (Motive 5본 스파인 체인)
|
|
// Motive: Hip → Ab → Spine2 → Spine3 → Spine4 → Chest → Neck → Neck2 → Head
|
|
// FBX: Hips → Spine → Spine1 → Spine2 → Spine3 → Spine4 → Neck → Neck1 → Head
|
|
{"Hip", "Hips"},
|
|
{"Ab", "Spine"},
|
|
{"Spine2", "Spine1"},
|
|
{"Spine3", "Spine2"},
|
|
{"Spine4", "Spine3"},
|
|
{"Chest", "Spine4"},
|
|
{"Neck", "Neck"},
|
|
{"Neck2", "Neck1"},
|
|
{"Head", "Head"},
|
|
|
|
// 왼쪽 팔
|
|
{"LShoulder", "LeftShoulder"},
|
|
{"LUArm", "LeftArm"},
|
|
{"LFArm", "LeftForeArm"},
|
|
{"LHand", "LeftHand"},
|
|
|
|
// 오른쪽 팔
|
|
{"RShoulder", "RightShoulder"},
|
|
{"RUArm", "RightArm"},
|
|
{"RFArm", "RightForeArm"},
|
|
{"RHand", "RightHand"},
|
|
|
|
// 왼쪽 다리
|
|
{"LThigh", "LeftUpLeg"},
|
|
{"LShin", "LeftLeg"},
|
|
{"LFoot", "LeftFoot"},
|
|
{"LToe", "LeftToeBase"},
|
|
|
|
// 오른쪽 다리
|
|
{"RThigh", "RightUpLeg"},
|
|
{"RShin", "RightLeg"},
|
|
{"RFoot", "RightFoot"},
|
|
{"RToe", "RightToeBase"},
|
|
|
|
// 왼쪽 손가락
|
|
{"LThumb1", "LeftHandThumb1"},
|
|
{"LThumb2", "LeftHandThumb2"},
|
|
{"LThumb3", "LeftHandThumb3"},
|
|
{"LIndex1", "LeftHandIndex1"},
|
|
{"LIndex2", "LeftHandIndex2"},
|
|
{"LIndex3", "LeftHandIndex3"},
|
|
{"LMiddle1", "LeftHandMiddle1"},
|
|
{"LMiddle2", "LeftHandMiddle2"},
|
|
{"LMiddle3", "LeftHandMiddle3"},
|
|
{"LRing1", "LeftHandRing1"},
|
|
{"LRing2", "LeftHandRing2"},
|
|
{"LRing3", "LeftHandRing3"},
|
|
{"LPinky1", "LeftHandPinky1"},
|
|
{"LPinky2", "LeftHandPinky2"},
|
|
{"LPinky3", "LeftHandPinky3"},
|
|
|
|
// 오른쪽 손가락
|
|
{"RThumb1", "RightHandThumb1"},
|
|
{"RThumb2", "RightHandThumb2"},
|
|
{"RThumb3", "RightHandThumb3"},
|
|
{"RIndex1", "RightHandIndex1"},
|
|
{"RIndex2", "RightHandIndex2"},
|
|
{"RIndex3", "RightHandIndex3"},
|
|
{"RMiddle1", "RightHandMiddle1"},
|
|
{"RMiddle2", "RightHandMiddle2"},
|
|
{"RMiddle3", "RightHandMiddle3"},
|
|
{"RRing1", "RightHandRing1"},
|
|
{"RRing2", "RightHandRing2"},
|
|
{"RRing3", "RightHandRing3"},
|
|
{"RPinky1", "RightHandPinky1"},
|
|
{"RPinky2", "RightHandPinky2"},
|
|
{"RPinky3", "RightHandPinky3"},
|
|
};
|
|
|
|
void Start()
|
|
{
|
|
BuildTransformCache();
|
|
InitializeStreamingClient();
|
|
|
|
// SO에서 본 매핑 + T-포즈 로드
|
|
if (RestPoseAsset != null && RestPoseAsset.boneMappings.Count > 0)
|
|
{
|
|
// 본 매핑: SO → MB 런타임 복사 (cachedTransform은 아래 RefreshTransformCache에서 채워짐)
|
|
boneMappings.Clear();
|
|
foreach (var src in RestPoseAsset.boneMappings)
|
|
{
|
|
boneMappings.Add(new OptiTrackBoneMapping
|
|
{
|
|
optiTrackBoneName = src.optiTrackBoneName,
|
|
fbxNodeName = src.fbxNodeName,
|
|
applyPosition = src.applyPosition,
|
|
applyRotation = src.applyRotation
|
|
});
|
|
}
|
|
RefreshTransformCache();
|
|
Debug.Log($"[OptiTrack] 본 매핑 에셋 로드 완료: {boneMappings.Count}개", this);
|
|
}
|
|
else if (boneMappings.Count > 0)
|
|
{
|
|
RefreshTransformCache();
|
|
}
|
|
else
|
|
{
|
|
Debug.LogWarning("[OptiTrack] 본 매핑이 비어있습니다. 설정 창에서 'FBX 분석 → 자동 매핑 생성'을 눌러주세요.", this);
|
|
}
|
|
|
|
// T-포즈 로드
|
|
if (RestPoseAsset != null && RestPoseAsset.restPoseEntries.Count > 0)
|
|
{
|
|
RestPoseAsset.LoadRestPoseIntoRuntime(m_restLocalRotations, m_restLocalPositions);
|
|
m_isRestPoseCached = true;
|
|
Debug.Log($"[OptiTrack] T-포즈 에셋 로드 완료: {m_restLocalRotations.Count}개 본", this);
|
|
}
|
|
|
|
previousSkeletonName = SkeletonAssetName;
|
|
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;
|
|
|
|
void Update()
|
|
{
|
|
if (StreamingClient == null)
|
|
{
|
|
InitializeStreamingClient();
|
|
return;
|
|
}
|
|
|
|
// 필터 활성화 상태 동기화 (프리셋 값은 SetFilterStrength()에서만 적용)
|
|
enableBoneFilter = filterStrength != FilterStrength.Off;
|
|
|
|
// MirrorMode 변경 감지 → 필터 상태 리셋 (불연속 튐 방지)
|
|
bool currentMirrorMode = StreamingClient != null && StreamingClient.MirrorMode;
|
|
if (currentMirrorMode != m_lastMirrorMode)
|
|
{
|
|
m_filterStates.Clear();
|
|
m_lastMirrorMode = currentMirrorMode;
|
|
}
|
|
|
|
// 스켈레톤 이름 변경 감지
|
|
if (previousSkeletonName != SkeletonAssetName)
|
|
{
|
|
StreamingClient.RegisterSkeleton(this, SkeletonAssetName);
|
|
m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
|
|
previousSkeletonName = SkeletonAssetName;
|
|
if (m_skeletonDef != null)
|
|
RebuildBoneIdMapping();
|
|
return;
|
|
}
|
|
|
|
if (m_skeletonDef == null) return;
|
|
|
|
// 락 보호 하에 스냅샷 복사 + NatNet 하드웨어 타임스탬프 수신
|
|
OptitrackHiResTimer.Timestamp frameTs;
|
|
if (!StreamingClient.FillBoneSnapshot(m_skeletonDef.Id, m_snapshotPositions, m_snapshotOrientations, out frameTs))
|
|
return;
|
|
|
|
// ── NatNet 실제 프레임 간격 계산 (하드웨어 타이머 — 렌더 프레임 등락과 완전 독립) ──
|
|
if (m_hasLastFrameTimestamp && frameTs.m_ticks != m_lastFrameTimestamp.m_ticks)
|
|
{
|
|
float measuredDt = frameTs.SecondsSince(m_lastFrameTimestamp);
|
|
if (measuredDt > 0.001f && measuredDt < 0.1f) // 1ms~100ms 범위만 신뢰 (이상값 무시)
|
|
m_natNetDt = Mathf.Lerp(m_natNetDt, measuredDt, 0.1f);
|
|
}
|
|
m_lastFrameTimestamp = frameTs;
|
|
m_hasLastFrameTimestamp = true;
|
|
|
|
// ── Pass 1: Raw 데이터 적용 → IK 포인트 월드 위치 캡처 ──────────────────
|
|
// 필터가 활성화되어 있을 때만 two-pass, 비활성이면 single-pass
|
|
if (enableBoneFilter)
|
|
{
|
|
// Raw 데이터로 모든 본 업데이트 (필터 없이)
|
|
foreach (var bone in m_skeletonDef.Bones)
|
|
{
|
|
if (!m_boneIdToMappingIndex.TryGetValue(bone.Id, out int mappingIdx))
|
|
continue;
|
|
var mapping = boneMappings[mappingIdx];
|
|
if (!mapping.isMapped || mapping.cachedTransform == null)
|
|
continue;
|
|
|
|
if (mapping.applyRotation && m_snapshotOrientations.TryGetValue(bone.Id, out Quaternion rawOri))
|
|
mapping.cachedTransform.localRotation = rawOri;
|
|
if (mapping.applyPosition && m_snapshotPositions.TryGetValue(bone.Id, out Vector3 rawPos))
|
|
mapping.cachedTransform.localPosition = rawPos;
|
|
}
|
|
|
|
// Raw 상태에서 IK 포인트 월드 위치/회전 캡처
|
|
foreach (var ikBone in k_IKPointBones)
|
|
{
|
|
Transform t = GetBoneTransform(ikBone);
|
|
if (t != null)
|
|
{
|
|
m_rawWorldPositions[ikBone] = t.position;
|
|
m_rawWorldRotations[ikBone] = t.rotation;
|
|
}
|
|
}
|
|
|
|
// Pass 2: 필터 적용된 데이터로 덮어쓰기
|
|
foreach (var bone in m_skeletonDef.Bones)
|
|
{
|
|
if (!m_boneIdToMappingIndex.TryGetValue(bone.Id, out int mappingIdx))
|
|
continue;
|
|
var mapping = boneMappings[mappingIdx];
|
|
if (!mapping.isMapped || mapping.cachedTransform == null)
|
|
continue;
|
|
|
|
if (mapping.applyRotation && m_snapshotOrientations.TryGetValue(bone.Id, out Quaternion finalOri))
|
|
{
|
|
finalOri = ApplyOneEuroOri(bone.Id, finalOri, m_natNetDt);
|
|
mapping.cachedTransform.localRotation = finalOri;
|
|
}
|
|
if (mapping.applyPosition && m_snapshotPositions.TryGetValue(bone.Id, out Vector3 finalPos))
|
|
{
|
|
finalPos = ApplyOneEuroPos(bone.Id, finalPos, m_natNetDt);
|
|
mapping.cachedTransform.localPosition = finalPos;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 필터 비활성: single-pass, raw = filtered
|
|
foreach (var bone in m_skeletonDef.Bones)
|
|
{
|
|
if (!m_boneIdToMappingIndex.TryGetValue(bone.Id, out int mappingIdx))
|
|
continue;
|
|
var mapping = boneMappings[mappingIdx];
|
|
if (!mapping.isMapped || mapping.cachedTransform == null)
|
|
continue;
|
|
|
|
if (mapping.applyRotation && m_snapshotOrientations.TryGetValue(bone.Id, out Quaternion finalOri))
|
|
mapping.cachedTransform.localRotation = finalOri;
|
|
if (mapping.applyPosition && m_snapshotPositions.TryGetValue(bone.Id, out Vector3 finalPos))
|
|
mapping.cachedTransform.localPosition = finalPos;
|
|
}
|
|
|
|
// 필터 없으면 현재 Transform 위치/회전이 곧 raw
|
|
foreach (var ikBone in k_IKPointBones)
|
|
{
|
|
Transform t = GetBoneTransform(ikBone);
|
|
if (t != null)
|
|
{
|
|
m_rawWorldPositions[ikBone] = t.position;
|
|
m_rawWorldRotations[ikBone] = t.rotation;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ── 어깨 증폭 + 상완 역보정 (1.0이면 스킵) ──
|
|
if (Mathf.Abs(shoulderAmplify - 1f) > 0.001f)
|
|
{
|
|
AmplifyShoulderWithCompensation("LShoulder", "LUArm", true);
|
|
AmplifyShoulderWithCompensation("RShoulder", "RUArm", false);
|
|
}
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// 어깨 회전을 rest pose 대비 증폭하고, 상완에서 추가 회전분을 상쇄하여 손 위치를 보존합니다.
|
|
/// </summary>
|
|
private float m_leftShoulderBlend = 0f;
|
|
private float m_rightShoulderBlend = 0f;
|
|
|
|
/// <param name="isLeft">true = 왼쪽(Z < 0일 때 증폭), false = 오른쪽(Z > 0일 때 증폭)</param>
|
|
private void AmplifyShoulderWithCompensation(string shoulderName, string upperArmName, bool isLeft)
|
|
{
|
|
Transform shoulder = GetMappedTransform(shoulderName);
|
|
Transform upperArm = GetMappedTransform(upperArmName);
|
|
if (shoulder == null || upperArm == null) return;
|
|
|
|
// rest pose 대비 델타 추출
|
|
Quaternion restRot = GetRestLocalRotation(shoulderName);
|
|
Quaternion currentRot = shoulder.localRotation;
|
|
Quaternion delta = Quaternion.Inverse(restRot) * currentRot;
|
|
|
|
// 어깨가 올라가는 방향일 때만 증폭 (왼쪽: Z < 0, 오른쪽: Z > 0)
|
|
Vector3 deltaEuler = delta.eulerAngles;
|
|
float z = deltaEuler.z > 180f ? deltaEuler.z - 360f : deltaEuler.z;
|
|
float targetBlend = isLeft ? Mathf.Clamp01(-z / 5f) : Mathf.Clamp01(z / 5f);
|
|
|
|
// 부드러운 블렌딩 (급격한 on/off 방지)
|
|
ref float blend = ref (isLeft ? ref m_leftShoulderBlend : ref m_rightShoulderBlend);
|
|
blend = Mathf.Lerp(blend, targetBlend, 10f * Time.deltaTime);
|
|
|
|
if (blend < 0.001f) return;
|
|
|
|
// 증폭량: 1(원본) ~ shoulderAmplify 사이를 blend로 보간
|
|
float effectiveAmplify = Mathf.Lerp(1f, shoulderAmplify, blend);
|
|
Quaternion amplifiedDelta = Quaternion.SlerpUnclamped(Quaternion.identity, delta, effectiveAmplify);
|
|
Quaternion amplifiedRot = restRot * amplifiedDelta;
|
|
|
|
// 추가된 회전량
|
|
Quaternion extraRotation = Quaternion.Inverse(currentRot) * amplifiedRot;
|
|
|
|
// 어깨에 증폭 적용
|
|
shoulder.localRotation = amplifiedRot;
|
|
|
|
// 상완에서 추가분 상쇄 (손 위치 보존)
|
|
upperArm.localRotation = Quaternion.Inverse(extraRotation) * upperArm.localRotation;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 하이어라키의 모든 Transform을 이름으로 캐싱
|
|
/// </summary>
|
|
private void BuildTransformCache()
|
|
{
|
|
m_allTransforms.Clear();
|
|
var allChildren = GetComponentsInChildren<Transform>(true);
|
|
foreach (var t in allChildren)
|
|
{
|
|
if (!m_allTransforms.ContainsKey(t.name))
|
|
{
|
|
m_allTransforms[t.name] = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 기존 매핑의 Transform 캐시만 갱신
|
|
/// </summary>
|
|
private void RefreshTransformCache()
|
|
{
|
|
foreach (var mapping in boneMappings)
|
|
{
|
|
if (!string.IsNullOrEmpty(mapping.fbxNodeName) && m_allTransforms.TryGetValue(mapping.fbxNodeName, out Transform t))
|
|
{
|
|
mapping.cachedTransform = t;
|
|
mapping.isMapped = true;
|
|
}
|
|
}
|
|
RebuildOptiNameCache();
|
|
}
|
|
|
|
/// <summary>
|
|
/// OptiTrack 본 이름 → Transform 딕셔너리 캐시 구축 (GetMappedTransform O(n) → O(1))
|
|
/// </summary>
|
|
private void RebuildOptiNameCache()
|
|
{
|
|
m_optiNameTransformCache.Clear();
|
|
foreach (var mapping in boneMappings)
|
|
{
|
|
if (!string.IsNullOrEmpty(mapping.optiTrackBoneName) && mapping.isMapped && mapping.cachedTransform != null)
|
|
m_optiNameTransformCache[mapping.optiTrackBoneName] = mapping.cachedTransform;
|
|
}
|
|
RebuildChainCaches();
|
|
}
|
|
|
|
private void RebuildChainCaches()
|
|
{
|
|
m_spineChainCache.Clear();
|
|
foreach (var name in SpineChainOptiNames)
|
|
if (m_optiNameTransformCache.TryGetValue(name, out Transform t))
|
|
m_spineChainCache.Add(t);
|
|
|
|
m_neckChainCache.Clear();
|
|
foreach (var name in NeckChainOptiNames)
|
|
if (m_optiNameTransformCache.TryGetValue(name, out Transform t))
|
|
m_neckChainCache.Add(t);
|
|
}
|
|
|
|
/// <summary>
|
|
/// OptiTrack 본 ID → boneMappings 인덱스 매핑 구축
|
|
/// </summary>
|
|
private void RebuildBoneIdMapping()
|
|
{
|
|
m_boneIdToMappingIndex.Clear();
|
|
m_filterStates.Clear();
|
|
m_hasLastFrameTimestamp = false;
|
|
if (m_skeletonDef == null) return;
|
|
|
|
var nameToIdx = new Dictionary<string, int>();
|
|
for (int i = 0; i < boneMappings.Count; i++)
|
|
{
|
|
if (!nameToIdx.ContainsKey(boneMappings[i].optiTrackBoneName))
|
|
nameToIdx[boneMappings[i].optiTrackBoneName] = i;
|
|
}
|
|
|
|
int matchCount = 0;
|
|
m_hipBoneId = -1;
|
|
foreach (var bone in m_skeletonDef.Bones)
|
|
{
|
|
string boneName = bone.Name;
|
|
string optiName = boneName.Contains("_") ? boneName.Substring(boneName.IndexOf('_') + 1) : boneName;
|
|
if (nameToIdx.TryGetValue(optiName, out int idx))
|
|
{
|
|
m_boneIdToMappingIndex[bone.Id] = idx;
|
|
matchCount++;
|
|
}
|
|
if (optiName == "Hip") m_hipBoneId = bone.Id;
|
|
}
|
|
|
|
Debug.Log($"[OptiTrack] 본 ID 매핑 완료: {matchCount}/{m_skeletonDef.Bones.Count} 매칭");
|
|
}
|
|
|
|
private void InitializeStreamingClient()
|
|
{
|
|
if (StreamingClient == null)
|
|
{
|
|
StreamingClient = FindAnyObjectByType<OptitrackStreamingClient>();
|
|
if (StreamingClient != null)
|
|
Debug.Log("OptiTrack Streaming Client를 찾았습니다.", this);
|
|
}
|
|
}
|
|
|
|
private IEnumerator CheckSkeletonConnectionPeriodically()
|
|
{
|
|
while (true)
|
|
{
|
|
if (StreamingClient != null)
|
|
{
|
|
m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
|
|
|
|
if (m_skeletonDef != null)
|
|
{
|
|
OptitrackSkeletonState skelState = StreamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
|
|
bool wasFound = isSkeletonFound;
|
|
isSkeletonFound = (skelState != null);
|
|
|
|
if (isSkeletonFound && !wasFound)
|
|
{
|
|
StreamingClient.RegisterSkeleton(this, SkeletonAssetName);
|
|
previousSkeletonName = SkeletonAssetName;
|
|
RebuildBoneIdMapping();
|
|
Debug.Log($"[OptiTrack] 스켈레톤 '{SkeletonAssetName}' 연결 성공");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 스켈레톤 정의를 찾지 못함 → 서버에 정의 재조회 요청
|
|
isSkeletonFound = false;
|
|
StreamingClient.RequestDefinitionRefresh();
|
|
}
|
|
}
|
|
|
|
yield return new WaitForSeconds(k_SkeletonCheckInterval);
|
|
}
|
|
}
|
|
|
|
// ── 1€ Filter (One Euro Filter) ──────────────────────────────────────────
|
|
// 참고: Géry Casiez et al., "1€ Filter: A Simple Speed-based Low-pass Filter", CHI 2012
|
|
// 속도가 빠를수록 cutoff 상승 → 지연 감소, 속도가 느릴수록 cutoff = minCutoff → 노이즈 제거
|
|
|
|
// dt초 간격에서의 1차 LP 필터 alpha
|
|
private static float OE_Alpha(float cutoffHz, float dt)
|
|
{
|
|
float r = 2f * Mathf.PI * cutoffHz * dt;
|
|
return r / (r + 1f);
|
|
}
|
|
|
|
private Quaternion ApplyOneEuroOri(int boneId, Quaternion raw, float dt)
|
|
{
|
|
const float k_DCutoff = 1f; // 도함수 필터의 고정 cutoff (Hz)
|
|
|
|
if (!m_filterStates.TryGetValue(boneId, out BoneFilterState s) || !s.initialized)
|
|
{
|
|
m_filterStates[boneId] = new BoneFilterState
|
|
{
|
|
prevOri = raw, dOriMag = 0f,
|
|
prevPos = Vector3.zero, dPosMag = 0f,
|
|
initialized = true
|
|
};
|
|
return raw;
|
|
}
|
|
|
|
// 각속도(rad/s) 추정
|
|
float angleDeg = Quaternion.Angle(s.prevOri, raw);
|
|
float speed = angleDeg * Mathf.Deg2Rad / Mathf.Max(dt, 1e-4f);
|
|
|
|
// 도함수를 별도 LP로 스무딩
|
|
float dAlpha = OE_Alpha(k_DCutoff, dt);
|
|
float filtDeriv = s.dOriMag + dAlpha * (speed - s.dOriMag);
|
|
|
|
// 적응 cutoff: 빠르면 cutoff 상승, 단 maxCutoff 이상은 항상 제거
|
|
float cutoff = Mathf.Min(filterMinCutoff + filterBeta * filtDeriv, filterMaxCutoff);
|
|
float alpha = OE_Alpha(cutoff, dt);
|
|
Quaternion filtered = Quaternion.Slerp(s.prevOri, raw, alpha);
|
|
|
|
s.prevOri = filtered;
|
|
s.dOriMag = filtDeriv;
|
|
m_filterStates[boneId] = s;
|
|
return filtered;
|
|
}
|
|
|
|
private Vector3 ApplyOneEuroPos(int boneId, Vector3 raw, float dt)
|
|
{
|
|
const float k_DCutoff = 1f;
|
|
|
|
if (!m_filterStates.TryGetValue(boneId, out BoneFilterState s) || !s.initialized)
|
|
{
|
|
// 회전 필터가 이미 초기화했을 수 있으므로 위치만 패치
|
|
s.prevPos = raw;
|
|
s.dPosMag = 0f;
|
|
s.initialized = true;
|
|
m_filterStates[boneId] = s;
|
|
return raw;
|
|
}
|
|
|
|
float speed = (raw - s.prevPos).magnitude / Mathf.Max(dt, 1e-4f);
|
|
float dAlpha = OE_Alpha(k_DCutoff, dt);
|
|
float filtDeriv = s.dPosMag + dAlpha * (speed - s.dPosMag);
|
|
|
|
float cutoff = Mathf.Min(filterMinCutoff + filterBeta * filtDeriv, filterMaxCutoff);
|
|
float alpha = OE_Alpha(cutoff, dt);
|
|
Vector3 filtered = Vector3.Lerp(s.prevPos, raw, alpha);
|
|
|
|
s.prevPos = filtered;
|
|
s.dPosMag = filtDeriv;
|
|
m_filterStates[boneId] = s;
|
|
return filtered;
|
|
}
|
|
|
|
#region 외부 접근용 헬퍼
|
|
|
|
// HumanBodyBones → OptiTrack 본 이름 매핑 (Humanoid 호환 레이어)
|
|
public static readonly Dictionary<HumanBodyBones, string> HumanBoneToOptiName = new Dictionary<HumanBodyBones, string>
|
|
{
|
|
{ HumanBodyBones.Hips, "Hip" },
|
|
{ HumanBodyBones.Head, "Head" },
|
|
// 스파인/넥은 분배 처리되므로 1:1 매핑은 참고용
|
|
{ HumanBodyBones.Spine, "Ab" },
|
|
{ HumanBodyBones.Chest, "Chest" },
|
|
{ HumanBodyBones.Neck, "Neck" },
|
|
// 왼쪽 팔
|
|
{ HumanBodyBones.LeftShoulder, "LShoulder" },
|
|
{ HumanBodyBones.LeftUpperArm, "LUArm" },
|
|
{ HumanBodyBones.LeftLowerArm, "LFArm" },
|
|
{ HumanBodyBones.LeftHand, "LHand" },
|
|
// 오른쪽 팔
|
|
{ HumanBodyBones.RightShoulder, "RShoulder" },
|
|
{ HumanBodyBones.RightUpperArm, "RUArm" },
|
|
{ HumanBodyBones.RightLowerArm, "RFArm" },
|
|
{ HumanBodyBones.RightHand, "RHand" },
|
|
// 왼쪽 다리
|
|
{ HumanBodyBones.LeftUpperLeg, "LThigh" },
|
|
{ HumanBodyBones.LeftLowerLeg, "LShin" },
|
|
{ HumanBodyBones.LeftFoot, "LFoot" },
|
|
{ HumanBodyBones.LeftToes, "LToe" },
|
|
// 오른쪽 다리
|
|
{ HumanBodyBones.RightUpperLeg, "RThigh" },
|
|
{ HumanBodyBones.RightLowerLeg, "RShin" },
|
|
{ HumanBodyBones.RightFoot, "RFoot" },
|
|
{ HumanBodyBones.RightToes, "RToe" },
|
|
// 왼쪽 손가락
|
|
{ HumanBodyBones.LeftThumbProximal, "LThumb1" },
|
|
{ HumanBodyBones.LeftThumbIntermediate, "LThumb2" },
|
|
{ HumanBodyBones.LeftThumbDistal, "LThumb3" },
|
|
{ HumanBodyBones.LeftIndexProximal, "LIndex1" },
|
|
{ HumanBodyBones.LeftIndexIntermediate, "LIndex2" },
|
|
{ HumanBodyBones.LeftIndexDistal, "LIndex3" },
|
|
{ HumanBodyBones.LeftMiddleProximal, "LMiddle1" },
|
|
{ HumanBodyBones.LeftMiddleIntermediate, "LMiddle2" },
|
|
{ HumanBodyBones.LeftMiddleDistal, "LMiddle3" },
|
|
{ HumanBodyBones.LeftRingProximal, "LRing1" },
|
|
{ HumanBodyBones.LeftRingIntermediate, "LRing2" },
|
|
{ HumanBodyBones.LeftRingDistal, "LRing3" },
|
|
{ HumanBodyBones.LeftLittleProximal, "LPinky1" },
|
|
{ HumanBodyBones.LeftLittleIntermediate, "LPinky2" },
|
|
{ HumanBodyBones.LeftLittleDistal, "LPinky3" },
|
|
// 오른쪽 손가락
|
|
{ HumanBodyBones.RightThumbProximal, "RThumb1" },
|
|
{ HumanBodyBones.RightThumbIntermediate, "RThumb2" },
|
|
{ HumanBodyBones.RightThumbDistal, "RThumb3" },
|
|
{ HumanBodyBones.RightIndexProximal, "RIndex1" },
|
|
{ HumanBodyBones.RightIndexIntermediate, "RIndex2" },
|
|
{ HumanBodyBones.RightIndexDistal, "RIndex3" },
|
|
{ HumanBodyBones.RightMiddleProximal, "RMiddle1" },
|
|
{ HumanBodyBones.RightMiddleIntermediate, "RMiddle2" },
|
|
{ HumanBodyBones.RightMiddleDistal, "RMiddle3" },
|
|
{ HumanBodyBones.RightRingProximal, "RRing1" },
|
|
{ HumanBodyBones.RightRingIntermediate, "RRing2" },
|
|
{ HumanBodyBones.RightRingDistal, "RRing3" },
|
|
{ HumanBodyBones.RightLittleProximal, "RPinky1" },
|
|
{ HumanBodyBones.RightLittleIntermediate, "RPinky2" },
|
|
{ HumanBodyBones.RightLittleDistal, "RPinky3" },
|
|
};
|
|
|
|
/// <summary>
|
|
/// 1€ 필터 적용 전의 raw 월드 위치를 반환합니다.
|
|
/// IK 타겟(발 접지 등)에 사용하면 필터 스무딩으로 인한 접지력 저하를 방지합니다.
|
|
/// 지원 본: LeftFoot, RightFoot, LeftHand, RightHand, LeftToes, RightToes
|
|
/// </summary>
|
|
public bool TryGetRawWorldPosition(HumanBodyBones bone, out Vector3 position)
|
|
{
|
|
return m_rawWorldPositions.TryGetValue(bone, out position);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 1€ 필터 적용 전의 raw 월드 회전을 반환합니다.
|
|
/// </summary>
|
|
public bool TryGetRawWorldRotation(HumanBodyBones bone, out Quaternion rotation)
|
|
{
|
|
return m_rawWorldRotations.TryGetValue(bone, out rotation);
|
|
}
|
|
|
|
/// <summary>
|
|
/// HumanBodyBones enum으로 OptiTrack 매핑된 Transform 반환
|
|
/// (Humanoid 호환 레이어 — sourceAnimator.GetBoneTransform() 대체)
|
|
/// </summary>
|
|
public Transform GetBoneTransform(HumanBodyBones bone)
|
|
{
|
|
if (HumanBoneToOptiName.TryGetValue(bone, out string optiName))
|
|
return GetMappedTransform(optiName);
|
|
return null;
|
|
}
|
|
|
|
// 스파인 체인 OptiTrack 이름 (Hips 제외, 순서대로)
|
|
public static readonly string[] SpineChainOptiNames = { "Ab", "Spine2", "Spine3", "Spine4", "Chest" };
|
|
// 넥 체인
|
|
public static readonly string[] NeckChainOptiNames = { "Neck", "Neck2" };
|
|
|
|
/// <summary>
|
|
/// OptiTrack 본 이름으로 매핑된 Transform 반환 (캐시 O(1), 미캐시 시 선형 탐색 폴백)
|
|
/// </summary>
|
|
public Transform GetMappedTransform(string optiTrackBoneName)
|
|
{
|
|
if (m_optiNameTransformCache.TryGetValue(optiTrackBoneName, out var cached))
|
|
return cached;
|
|
// 캐시 미구축 상태 폴백 (Start() 전 호출 등)
|
|
foreach (var mapping in boneMappings)
|
|
{
|
|
if (mapping.optiTrackBoneName == optiTrackBoneName && mapping.isMapped)
|
|
return mapping.cachedTransform;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 스파인 체인의 모든 Transform을 순서대로 반환 (Ab → Spine2 → Spine3 → Spine4 → Chest)
|
|
/// </summary>
|
|
public List<Transform> GetSpineChainTransforms() => m_spineChainCache;
|
|
|
|
/// <summary>
|
|
/// 넥 체인의 모든 Transform을 순서대로 반환
|
|
/// </summary>
|
|
public List<Transform> GetNeckChainTransforms() => m_neckChainCache;
|
|
|
|
/// <summary>
|
|
/// 체인의 누적 로컬 회전을 계산 (각 본의 localRotation을 순서대로 곱함)
|
|
/// </summary>
|
|
public static Quaternion ComputeChainRotation(List<Transform> chain)
|
|
{
|
|
Quaternion total = Quaternion.identity;
|
|
foreach (var t in chain)
|
|
{
|
|
total *= t.localRotation;
|
|
}
|
|
return total;
|
|
}
|
|
|
|
// T-포즈 기준 포즈 데이터 (SO에서 로드 또는 CacheRestPose()로 캐싱)
|
|
private Dictionary<string, Quaternion> m_restLocalRotations = new Dictionary<string, Quaternion>();
|
|
private Dictionary<string, Vector3> m_restLocalPositions = new Dictionary<string, Vector3>();
|
|
private bool m_isRestPoseCached = false;
|
|
|
|
/// <summary>
|
|
/// 현재 포즈를 기준 포즈(T-포즈)로 캐싱
|
|
/// </summary>
|
|
public void CacheRestPose()
|
|
{
|
|
m_restLocalRotations.Clear();
|
|
m_restLocalPositions.Clear();
|
|
|
|
foreach (var mapping in boneMappings)
|
|
{
|
|
if (mapping.isMapped && mapping.cachedTransform != null)
|
|
{
|
|
m_restLocalRotations[mapping.optiTrackBoneName] = mapping.cachedTransform.localRotation;
|
|
m_restLocalPositions[mapping.optiTrackBoneName] = mapping.cachedTransform.localPosition;
|
|
}
|
|
}
|
|
m_isRestPoseCached = true;
|
|
Debug.Log($"[OptiTrack] 기준 포즈 캐싱 완료: {m_restLocalRotations.Count}개 본");
|
|
|
|
// SO가 연결되어 있으면 런타임 결과를 SO에도 반영
|
|
if (RestPoseAsset != null)
|
|
{
|
|
RestPoseAsset.restPoseEntries.Clear();
|
|
foreach (var kvp in m_restLocalRotations)
|
|
{
|
|
m_restLocalPositions.TryGetValue(kvp.Key, out Vector3 pos);
|
|
RestPoseAsset.restPoseEntries.Add(new OptitrackRestPoseData.RestPoseEntry
|
|
{
|
|
boneName = kvp.Key,
|
|
localPosition = pos,
|
|
localRotation = kvp.Value
|
|
});
|
|
}
|
|
#if UNITY_EDITOR
|
|
UnityEditor.EditorUtility.SetDirty(RestPoseAsset);
|
|
UnityEditor.AssetDatabase.SaveAssets();
|
|
#endif
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캐싱된 기준 포즈로 복원 (T-포즈 복원)
|
|
/// </summary>
|
|
public void RestoreRestPose()
|
|
{
|
|
if (!m_isRestPoseCached) return;
|
|
|
|
foreach (var mapping in boneMappings)
|
|
{
|
|
if (mapping.isMapped && mapping.cachedTransform != null)
|
|
{
|
|
if (m_restLocalRotations.TryGetValue(mapping.optiTrackBoneName, out Quaternion rot))
|
|
mapping.cachedTransform.localRotation = rot;
|
|
if (m_restLocalPositions.TryGetValue(mapping.optiTrackBoneName, out Vector3 pos))
|
|
mapping.cachedTransform.localPosition = pos;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 특정 본의 기준 포즈 로컬 회전 반환
|
|
/// </summary>
|
|
public Quaternion GetRestLocalRotation(string optiTrackBoneName)
|
|
{
|
|
if (m_restLocalRotations.TryGetValue(optiTrackBoneName, out Quaternion rot))
|
|
return rot;
|
|
return Quaternion.identity;
|
|
}
|
|
|
|
#endregion
|
|
}
|