From 4b31d6de06c0fea90ae65992a6a2a612857b8f50 Mon Sep 17 00:00:00 2001 From: "qsxft258@gmail.com" Date: Tue, 2 Jun 2026 23:23:10 +0900 Subject: [PATCH] =?UTF-8?q?Fix=20:=20=EC=98=B5=ED=8B=B0=20=EC=8A=A4?= =?UTF-8?q?=ED=81=AC=EB=A6=BD=ED=8A=B8=20=EC=97=85=EB=8D=B0=EC=9D=B4?= =?UTF-8?q?=ED=8A=B8=20=EB=B0=8F=20=ED=94=84=EB=A6=AC=ED=8E=A9=20=EC=97=85?= =?UTF-8?q?=EB=8D=B0=EC=9D=B4=ED=8A=B8=20=EB=B0=8F=20=ED=95=84=ED=84=B0=20?= =?UTF-8?q?=EC=8B=9C=EC=8A=A4=ED=85=9C=EC=B6=94=EA=B0=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Prefabs/Client - OptiTrack.prefab | 4 +- .../Editor/OptitrackStreamingClientEditor.cs | 4 +- .../UXML/OptitrackStreamingClientEditor.uxml | 1 - .../OptiTrack/Scripts/OptitrackFaceDevice.cs | 15 +- .../Scripts/OptitrackStreamingClient.cs | 314 ++++++++++++++---- .../Scripts/OptitrackTrainedMarkerset.cs | 39 +-- 6 files changed, 274 insertions(+), 103 deletions(-) diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Prefabs/Client - OptiTrack.prefab b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Prefabs/Client - OptiTrack.prefab index aff0f2476..63586374a 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Prefabs/Client - OptiTrack.prefab +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Prefabs/Client - OptiTrack.prefab @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7e18b0e4238d5e7399f208845a9417f66a2b7719a71d9bdf5c7362eab9954ab1 -size 1724 +oid sha256:8fc5f2f318752bfee1302d10448847f4d37cf312d296bf80929f91ace8b33d87 +size 1798 diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/OptitrackStreamingClientEditor.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/OptitrackStreamingClientEditor.cs index 9ea667607..ce0c5c5d6 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/OptitrackStreamingClientEditor.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/OptitrackStreamingClientEditor.cs @@ -121,7 +121,9 @@ public class OptitrackStreamingClientEditor : Editor runtimeInfo.Clear(); AddInfoRow(runtimeInfo, "Server Address", client.ServerAddress); - AddInfoRow(runtimeInfo, "Local Address", client.LocalAddress); + AddInfoRow(runtimeInfo, "Local Address", string.IsNullOrEmpty(client.ResolvedLocalAddress) + ? client.LocalAddress + : client.ResolvedLocalAddress); AddInfoRow(runtimeInfo, "Connection Type", client.ConnectionType.ToString()); if (!string.IsNullOrEmpty(client.ServerNatNetVersion)) diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml index 003eb81ba..7288d631b 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml @@ -13,7 +13,6 @@ - diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackFaceDevice.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackFaceDevice.cs index 2d6279ea0..713d9c293 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackFaceDevice.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackFaceDevice.cs @@ -66,13 +66,14 @@ public class OptitrackFaceDevice : MonoBehaviour // Resolved device names (1 or 2: base, base_A+_B) private List _resolvedDeviceNames = new List(2); + private Dictionary _channelSnapshot = new Dictionary(); private float _lastResolveAttempt = -10f; private const float k_ResolveRetrySeconds = 1.0f; void Start() { if (streamingClient == null) - streamingClient = FindObjectOfType(); + streamingClient = FindFirstObjectByType(); if (streamingClient != null && !streamingClient.ReceiveDevices) { @@ -163,16 +164,10 @@ public class OptitrackFaceDevice : MonoBehaviour int appliedBs = 0; for (int i = 0; i < _resolvedDeviceNames.Count; ++i) { - var state = streamingClient.GetLatestDeviceState(_resolvedDeviceNames[i]); - if (state == null) continue; + if (!streamingClient.FillDeviceChannelSnapshot(_resolvedDeviceNames[i], _channelSnapshot)) + continue; - // Snapshot to a local list to avoid holding the lock during apply. - // ChannelValues is a Dictionary updated by the NatNet thread; iterating - // it while it mutates would throw. The streaming client's GetLatestDeviceState - // returns the live reference, so we iterate quickly under no lock — accept - // occasional torn reads (one frame stale value) over the perf cost of - // copying the dict every frame. - foreach (var kv in state.ChannelValues) + foreach (var kv in _channelSnapshot) { string name = kv.Key; float v = kv.Value; diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs index 0b6b6a4e6..d0bd2be5f 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs @@ -17,6 +17,7 @@ limitations under the License. using System; using System.Collections.Generic; using System.Net; +using System.Net.Sockets; using System.Runtime.InteropServices; using System.Threading; using UnityEngine; @@ -312,8 +313,17 @@ public class OptitrackStreamingClient : MonoBehaviour public string ServerAddress = "127.0.0.1"; [Tooltip("Must be on the same network as the Streaming IP (Local Interface) in Motive.")] + [HideInInspector] public string LocalAddress = "127.0.0.1"; + [Tooltip("Automatically selects the local IPv4 interface that routes to ServerAddress. LocalAddress is used as a fallback if detection fails.")] + [HideInInspector] + public bool AutoDetectLocalAddress = true; + + [Tooltip("The local IPv4 address selected for the active connection.")] + [HideInInspector] + public string ResolvedLocalAddress = ""; + [Tooltip("Unicast performs subscription reducing your overall data set in some applications.")] public ClientConnectionType ConnectionType; @@ -340,7 +350,7 @@ public class OptitrackStreamingClient : MonoBehaviour [Tooltip("Draws force plate visuals in the viewport for debugging and other uses.")] public bool DrawForcePlates = false; - [Tooltip("Receive analog device data (e.g. iFacialMocap face streams). Each frame the full sFrameOfMocapData is marshaled (~200KB), so leave off if no devices are needed.")] + [Tooltip("Receive analog device data (e.g. iFacialMocap face streams). Leave off if no devices are needed.")] public bool ReceiveDevices = false; [Tooltip("Motive will record when the Unity project is played.")] @@ -401,6 +411,9 @@ public class OptitrackStreamingClient : MonoBehaviour /// Maps from a streamed skeleton's ID to its most recent available pose data. private Dictionary m_latestSkeletonStates = new Dictionary(); + /// Reusable staging buffers used to validate a complete skeleton frame before committing it. + private Dictionary m_skeletonFrameScratch = new Dictionary(); + /// MirrorMode용: 스켈레톤 ID → (boneId → mirrorBoneId) 매핑 캐시. private Dictionary> m_mirrorBoneIdMaps = new Dictionary>(); @@ -771,6 +784,7 @@ public class OptitrackStreamingClient : MonoBehaviour for (int attempt = 1; attempt <= maxAttempts; attempt++) { + m_receivedFrameSinceConnect = false; float delay = (attempt == 1) ? 1.0f : 5.0f; Debug.Log(string.Format("{0}: 재연결 시도 {1}/{2} — {3}초 후...", GetType().FullName, attempt, maxAttempts, delay), this); yield return new WaitForSeconds(delay); @@ -1103,30 +1117,62 @@ public class OptitrackStreamingClient : MonoBehaviour /// The most recent available state, or null if none available. public OptitrackTMarkersetState GetLatestTMarkersetState(Int32 tmarkersetId) { - OptitrackTMarkersetState tmarState; - lock (m_frameDataUpdateLock) { - m_latestTMarkersetStates.TryGetValue(tmarkersetId, out tmarState); - } + if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var source) || source == null) + return null; - if ( MirrorMode && tmarState != null && tmarState.BonePoses != null ) - { - var mirrored = new OptitrackTMarkersetState + var snapshot = new OptitrackTMarkersetState { - BonePoses = new Dictionary( tmarState.BonePoses.Count ), - LocalBonePoses = tmarState.LocalBonePoses, + BonePoses = CopyPoseDictionary(source.BonePoses, MirrorMode), + LocalBonePoses = CopyPoseDictionary(source.LocalBonePoses, MirrorMode), }; - foreach ( var kvp in tmarState.BonePoses ) - mirrored.BonePoses[kvp.Key] = new OptitrackPose - { - Position = MirrorPosition( kvp.Value.Position ), - Orientation = MirrorOrientation( kvp.Value.Orientation ), - }; - tmarState = mirrored; + return snapshot; } + } - return tmarState; + private static Dictionary CopyPoseDictionary( + Dictionary source, bool mirror) + { + var snapshot = new Dictionary(source != null ? source.Count : 0); + if (source == null) + return snapshot; + + foreach (var kvp in source) + { + snapshot[kvp.Key] = new OptitrackPose + { + Position = mirror ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position, + Orientation = mirror ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation, + }; + } + return snapshot; + } + + /// + /// Copies trained markerset poses while holding the NatNet frame lock. + /// + public bool FillTMarkersetSnapshot(Int32 tmarkersetId, bool useLocalBonePoses, + Dictionary posOut, Dictionary oriOut) + { + lock (m_frameDataUpdateLock) + { + if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var state) || state == null) + return false; + + var source = useLocalBonePoses ? state.LocalBonePoses : state.BonePoses; + if (source == null) + return false; + + posOut.Clear(); + oriOut.Clear(); + foreach (var kvp in source) + { + posOut[kvp.Key] = MirrorMode ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position; + oriOut[kvp.Key] = MirrorMode ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation; + } + return true; + } } @@ -1550,42 +1596,38 @@ public class OptitrackStreamingClient : MonoBehaviour // ---------------------------------- // - Device Definitions (generic analog: iFacialMocap, NIDAQ, etc.) // ---------------------------------- - if (m_dataDescs.DeviceDescriptions != null) + lock (m_frameDataUpdateLock) { - for (int devIdx = 0; devIdx < m_dataDescs.DeviceDescriptions.Count; ++devIdx) + if (m_dataDescs.DeviceDescriptions != null) { - sDeviceDescription dev = m_dataDescs.DeviceDescriptions[devIdx]; - - // dev.ChannelNames is a fixed-size 32 array (NatNet C# wrapper limit). - // Plugin can register devices with > 32 channels (Motive stores them all), - // but only the first 32 names survive the wrapper marshaling. - // IMPORTANT: ChannelCount is preserved as the full count reported by Motive - // (could be > 32, e.g. iFacialMocap primary = 54). Downstream consumers - // need this to match the wire frame's nCh (which may also be > 32 if Motive - // doesn't truncate broadcast). ChannelNames is clamped to the marshaled - // array length to avoid IndexOutOfRangeException. - int marshalNameCount = (dev.ChannelNames != null) - ? Math.Min(dev.ChannelCount, dev.ChannelNames.Length) - : 0; - - OptitrackDeviceDefinition deviceDef = new OptitrackDeviceDefinition + for (int devIdx = 0; devIdx < m_dataDescs.DeviceDescriptions.Count; ++devIdx) { - Id = dev.Id, - Name = dev.Name, - SerialNumber = dev.SerialNo, - DeviceType = dev.DeviceType, - ChannelDataType = dev.ChannelDataType, - ChannelCount = dev.ChannelCount, - ChannelNames = new List(marshalNameCount), - }; + sDeviceDescription dev = m_dataDescs.DeviceDescriptions[devIdx]; - for (int i = 0; i < marshalNameCount; ++i) - { - deviceDef.ChannelNames.Add(dev.ChannelNames[i]); + // dev.ChannelNames is a fixed-size 32 array (NatNet C# wrapper limit). + int marshalNameCount = (dev.ChannelNames != null) + ? Math.Min(dev.ChannelCount, dev.ChannelNames.Length) + : 0; + + OptitrackDeviceDefinition deviceDef = new OptitrackDeviceDefinition + { + Id = dev.Id, + Name = dev.Name, + SerialNumber = dev.SerialNo, + DeviceType = dev.DeviceType, + ChannelDataType = dev.ChannelDataType, + ChannelCount = dev.ChannelCount, + ChannelNames = new List(marshalNameCount), + }; + + for (int i = 0; i < marshalNameCount; ++i) + { + deviceDef.ChannelNames.Add(dev.ChannelNames[i]); + } + + m_deviceDefinitions.Add(deviceDef); + m_deviceNameToId[deviceDef.Name] = deviceDef.Id; } - - m_deviceDefinitions.Add(deviceDef); - m_deviceNameToId[deviceDef.Name] = deviceDef.Id; } } @@ -1725,6 +1767,7 @@ public class OptitrackStreamingClient : MonoBehaviour private System.Collections.IEnumerator ConnectCoroutine() { + m_receivedFrameSinceConnect = false; IPAddress serverAddr; IPAddress localAddr; NatNetConnectionType connType; @@ -1732,7 +1775,8 @@ public class OptitrackStreamingClient : MonoBehaviour try { serverAddr = IPAddress.Parse( ServerAddress ); - localAddr = IPAddress.Parse( LocalAddress ); + localAddr = ResolveLocalAddress( serverAddr ); + ResolvedLocalAddress = localAddr.ToString(); connType = ConnectionType == ClientConnectionType.Unicast ? NatNetConnectionType.NatNetConnectionType_Unicast : NatNetConnectionType.NatNetConnectionType_Multicast; @@ -1795,6 +1839,8 @@ public class OptitrackStreamingClient : MonoBehaviour SubscribeSkeleton(skel.Value, skel.Key); foreach (KeyValuePair tmark in m_tmarkersets) // trained markerset added SubscribeTMarkerset(tmark.Value, tmark.Key); + if (DrawTMarkersetMarkers) + SubscribeTMarkMarkers(); } // 재연결 중에는 녹화 시작 스킵 — Motive의 Take 파일 반복 열기/닫기 방지 @@ -1954,13 +2000,6 @@ public class OptitrackStreamingClient : MonoBehaviour result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody( pFrame, rbIdx, out rbData ); NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBody failed." ); - bool bTrackedThisFrame = (rbData.Params & 0x01) != 0; - - if (bTrackedThisFrame == false) - { - continue; - } - // Ensure we have a state corresponding to this rigid body ID. OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState( rbData.Id ); RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState); @@ -1982,7 +2021,6 @@ public class OptitrackStreamingClient : MonoBehaviour // Ensure we have a state corresponding to this skeleton ID. OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId ); - skelState.DeliveryTimestamp = frameTimestamp; // Enumerate this skeleton's bone rigid bodies. Int32 skelRbCount; @@ -2002,17 +2040,47 @@ public class OptitrackStreamingClient : MonoBehaviour continue; } + // Motive can occasionally emit an empty or partial skeleton payload. Do not let + // one bad bone overwrite the previous valid pose: stage and validate first. + if (skelRbCount != skelDef.Bones.Count) + { + continue; + } + + sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, skelRbCount); + bool isValidSkeletonFrame = true; for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx) { - sRigidBodyData boneData = new sRigidBodyData(); - result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out boneData ); + 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]; // 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) || + !IsValidSkeletonBoneData(boneData)) + { + isValidSkeletonFrame = false; + break; + } + } + + if (!isValidSkeletonFrame) + { + 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; + NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId ); + // TODO: Could pre-populate this map when the definitions are retrieved. // Should never allocate after the first frame, at least. if (skelState.BonePoses.ContainsKey( boneId ) == false) @@ -2029,7 +2097,17 @@ public class OptitrackStreamingClient : MonoBehaviour Quaternion boneOri = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW); skelState.BonePoses[boneId].Position = bonePos; skelState.BonePoses[boneId].Orientation = boneOri; + } + // Derive locals in a second pass so parent order in the payload does not matter. + for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx) + { + sRigidBodyData boneData = stagedBones[boneIdx]; + Int32 boneSkelId, boneId; + NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId ); + + Vector3 bonePos = skelState.BonePoses[boneId].Position; + Quaternion boneOri = skelState.BonePoses[boneId].Orientation; Vector3 parentBonePos = new Vector3(0,0,0); Quaternion parentBoneOri = new Quaternion(0,0,0,1); @@ -2042,6 +2120,8 @@ public class OptitrackStreamingClient : MonoBehaviour skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos; skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri; } + + skelState.DeliveryTimestamp = frameTimestamp; } // ----------------------------------------------------- @@ -2050,6 +2130,7 @@ public class OptitrackStreamingClient : MonoBehaviour // m_dataDescs를 로컬 변수로 캡처: UpdateDefinitions()(메인 스레드)가 참조를 교체해도 안전 // null 체크: SkipDataDescriptions=true 또는 UpdateDefinitions() 미완료/실패 시 크래시 방지 var dataDescsSnapshot = m_dataDescs; + m_latestTMarkMarkerStates.Clear(); if (dataDescsSnapshot != null && dataDescsSnapshot.AssetDescriptions != null) for (int tmarkIdx = 0; tmarkIdx < dataDescsSnapshot.AssetDescriptions.Count; ++tmarkIdx) { @@ -2112,8 +2193,6 @@ public class OptitrackStreamingClient : MonoBehaviour NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarkerCount failed.");*/ //Debug.Log("tmark marker count: " + tmarkMarkerCount); // working finally - m_latestTMarkMarkerStates.Clear(); - // Update Trained Markerset Marker data for (int markerIdx = 0; markerIdx < tmarkMarkerCount; ++markerIdx) { @@ -2232,10 +2311,14 @@ public class OptitrackStreamingClient : MonoBehaviour devState.DeliveryTimestamp = frameTimestamp; int chCount = Math.Min(devData.ChannelCount, devDef.ChannelCount); + chCount = Math.Min(chCount, devData.ChannelData != null ? devData.ChannelData.Length : 0); + chCount = Math.Min(chCount, devDef.ChannelNames != null ? devDef.ChannelNames.Count : 0); for (int ch = 0; ch < chCount; ++ch) { // Use subframe 0 — face data is one sample per frame. sAnalogChannelData chData = devData.ChannelData[ch]; + if (chData.Values == null || chData.Values.Length == 0) + continue; float value = chData.Values[0]; devState.ChannelValues[devDef.ChannelNames[ch]] = value; } @@ -2270,7 +2353,7 @@ public class OptitrackStreamingClient : MonoBehaviour Monitor.Enter( m_frameDataUpdateLock ); try { - return m_latestDeviceStates.TryGetValue(deviceId, out var s) ? s : null; + return m_latestDeviceStates.TryGetValue(deviceId, out var state) ? CopyDeviceState(state) : null; } finally { @@ -2286,8 +2369,8 @@ public class OptitrackStreamingClient : MonoBehaviour try { if (m_deviceNameToId.TryGetValue(deviceName, out Int32 id) && - m_latestDeviceStates.TryGetValue(id, out var s)) - return s; + m_latestDeviceStates.TryGetValue(id, out var state)) + return CopyDeviceState(state); return null; } finally @@ -2296,6 +2379,38 @@ public class OptitrackStreamingClient : MonoBehaviour } } + private static OptitrackDeviceState CopyDeviceState(OptitrackDeviceState source) + { + if (source == null) + return null; + + return new OptitrackDeviceState + { + Id = source.Id, + Name = source.Name, + FrameNumber = source.FrameNumber, + DeliveryTimestamp = source.DeliveryTimestamp, + ChannelValues = new Dictionary(source.ChannelValues), + }; + } + + /// Copies a device's latest channel values into a caller-owned dictionary. + public bool FillDeviceChannelSnapshot(string deviceName, Dictionary channelValuesOut) + { + lock (m_frameDataUpdateLock) + { + channelValuesOut.Clear(); + if (!m_deviceNameToId.TryGetValue(deviceName, out Int32 id) || + !m_latestDeviceStates.TryGetValue(id, out var state) || + state == null) + return false; + + foreach (var kvp in state.ChannelValues) + channelValuesOut[kvp.Key] = kvp.Value; + return true; + } + } + /// Returns a snapshot of currently registered device definitions. public List GetDeviceDefinitions() @@ -2487,7 +2602,7 @@ public class OptitrackStreamingClient : MonoBehaviour { if (m_client != null && ConnectionType == ClientConnectionType.Unicast) { - bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData, TrainedMarkersetMarkers,All", 2000, 3); + bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData,TrainedMarkersetMarkers,All", 2000, 3); //Debug.Log("TMMarkers: " + subscribeSucceeded4); // Log a warning on the first failure. @@ -2557,6 +2672,71 @@ public class OptitrackStreamingClient : MonoBehaviour } } + private IPAddress ResolveLocalAddress(IPAddress serverAddress) + { + if (!AutoDetectLocalAddress) + return IPAddress.Parse(LocalAddress); + + if (IPAddress.IsLoopback(serverAddress)) + return IPAddress.Loopback; + + try + { + using (var socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp)) + { + socket.Connect(new IPEndPoint(serverAddress, NatNetConstants.DefaultCommandPort)); + if (socket.LocalEndPoint is IPEndPoint endpoint && + endpoint.Address.AddressFamily == AddressFamily.InterNetwork && + !IPAddress.Any.Equals(endpoint.Address)) + { + LocalAddress = endpoint.Address.ToString(); + return endpoint.Address; + } + } + } + catch (Exception ex) + { + Debug.LogWarning(GetType().FullName + ": 로컬 주소 자동 탐지 실패. LocalAddress fallback을 사용합니다. " + ex.Message, this); + } + + return IPAddress.Parse(LocalAddress); + } + + private sRigidBodyData[] GetSkeletonFrameScratch(Int32 skeletonId, Int32 boneCount) + { + if (!m_skeletonFrameScratch.TryGetValue(skeletonId, out var scratch) || scratch.Length != boneCount) + { + scratch = new sRigidBodyData[boneCount]; + m_skeletonFrameScratch[skeletonId] = scratch; + } + return scratch; + } + + private static bool IsValidSkeletonBoneData(sRigidBodyData boneData) + { + if ((boneData.Params & 0x01) == 0) + return false; + + float quaternionMagnitudeSquared = + boneData.QX * boneData.QX + + boneData.QY * boneData.QY + + boneData.QZ * boneData.QZ + + boneData.QW * boneData.QW; + + return IsFinite(boneData.X) && + IsFinite(boneData.Y) && + IsFinite(boneData.Z) && + IsFinite(boneData.QX) && + IsFinite(boneData.QY) && + IsFinite(boneData.QZ) && + IsFinite(boneData.QW) && + quaternionMagnitudeSquared > 0.000001f; + } + + private static bool IsFinite(float value) + { + return !float.IsNaN(value) && !float.IsInfinity(value); + } /// /// Returns the corresponding to the provided . @@ -2723,4 +2903,4 @@ public class OptitrackStreamingClient : MonoBehaviour Monitor.Exit( m_frameDataUpdateLock ); } #endregion Private methods -} \ No newline at end of file +} diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackTrainedMarkerset.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackTrainedMarkerset.cs index e4ce9bd3e..6b47154ae 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackTrainedMarkerset.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackTrainedMarkerset.cs @@ -1,5 +1,5 @@ /* -Copyright 2016 NaturalPoint Inc. +Copyright © 2016 NaturalPoint Inc. Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -53,6 +53,8 @@ public class OptitrackTrainedMarkerset : MonoBehaviour private Dictionary m_cachedBoneNameMap = new Dictionary(); // Optitrack's skeleton's bone names private Dictionary m_transformMap = new Dictionary(); + private Dictionary m_snapshotPositions = new Dictionary(); + private Dictionary m_snapshotOrientations = new Dictionary(); #endregion Private fields @@ -115,36 +117,29 @@ public class OptitrackTrainedMarkerset : MonoBehaviour private void Update() { - OptitrackTMarkersetState tmarState = StreamingClient.GetLatestTMarkersetState( m_tmarkersetDef.Id ); - if (tmarState != null) + bool useLocalBonePoses = StreamingClient.TMarkersetCoordinates == StreamingCoordinatesValues.Global; + if (StreamingClient.FillTMarkersetSnapshot(m_tmarkersetDef.Id, useLocalBonePoses, + m_snapshotPositions, m_snapshotOrientations)) { // Update the transforms of the bone GameObjects. for (int i = 0; i < m_tmarkersetDef.Bones.Count; ++i) { Int32 boneId = m_tmarkersetDef.Bones[i].Id; - OptitrackPose bonePose; GameObject boneObject; - bool foundPose = false; - if (StreamingClient.TMarkersetCoordinates == StreamingCoordinatesValues.Global) - { - // Use global tmarkerset coordinates - foundPose = tmarState.LocalBonePoses.TryGetValue(boneId, out bonePose); - } - else - { - // Use local tmarkerset coordinates - foundPose = tmarState.BonePoses.TryGetValue(boneId, out bonePose); - } - bool foundObject = m_boneObjectMap.TryGetValue(boneId, out boneObject); - if (foundPose && foundObject) + if (foundObject && + m_snapshotPositions.TryGetValue(boneId, out Vector3 position) && + m_snapshotOrientations.TryGetValue(boneId, out Quaternion orientation)) { - boneObject.transform.localPosition = bonePose.Position; - boneObject.transform.localRotation = bonePose.Orientation; - m_transformMap[boneObject.transform].transform.localPosition = bonePose.Position; - m_transformMap[boneObject.transform].transform.localRotation = bonePose.Orientation; + boneObject.transform.localPosition = position; + boneObject.transform.localRotation = orientation; + if (m_transformMap.TryGetValue(boneObject.transform, out Transform target)) + { + target.localPosition = position; + target.localRotation = orientation; + } } } } @@ -187,4 +182,4 @@ public class OptitrackTrainedMarkerset : MonoBehaviour } } #endregion Private methods -} \ No newline at end of file +}