diff --git a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs index 954a87f5d..15e4f1ee7 100644 --- a/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs +++ b/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs @@ -423,6 +423,15 @@ public class OptitrackStreamingClient : MonoBehaviour private UInt16 m_directCommandPort = 0; private UInt16 m_directDataPort = 0; + private enum DirectDefinitionSource + { + None, + Live, + Replay, + } + + private DirectDefinitionSource m_directDefinitionSource = DirectDefinitionSource.None; + private NatNetClient m_client; private NatNetClient m_replayClient; private bool m_replayReceivedFrameSinceConnect = false; @@ -492,6 +501,8 @@ public class OptitrackStreamingClient : MonoBehaviour private volatile bool m_pendingDefinitionRefresh = false; private volatile bool m_forceDefinitionRefreshSoon = false; + private volatile bool m_pendingReplayDefinitionRefresh = false; + private volatile bool m_forceReplayDefinitionRefreshSoon = false; private volatile bool m_pendingSkeletonDefinitionNotify = false; private float m_definitionRefreshCooldown = 0f; @@ -526,6 +537,15 @@ public class OptitrackStreamingClient : MonoBehaviour (m_client != null || (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected)); } + private bool CanRefreshReplayDefinitions() + { + return !SkipDataDescriptions && + EnableReplayPriority && + ConnectionType == ClientConnectionType.Multicast && + m_directNatNetConnected && + !string.IsNullOrWhiteSpace(ReplayServerAddress); + } + private void RefreshDefinitionsForActiveTransport() { if (m_client != null) @@ -545,6 +565,18 @@ public class OptitrackStreamingClient : MonoBehaviour throw new InvalidOperationException("No active OptiTrack transport is available for DataDescription refresh."); } + private void RefreshReplayDefinitionsForActiveTransport() + { + if (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected) + { + UpdateDirectReplayDefinitions(); + ResubscribeRegisteredAssets(); + return; + } + + throw new InvalidOperationException("No active MMRP replay transport is available for DataDescription refresh."); + } + private void Update() { if (m_pendingSkeletonDefinitionNotify) @@ -767,8 +799,32 @@ public class OptitrackStreamingClient : MonoBehaviour m_pendingDefinitionRefresh = true; m_definitionRefreshCooldown = 0f; } + if (m_forceReplayDefinitionRefreshSoon) + { + m_forceReplayDefinitionRefreshSoon = false; + m_pendingReplayDefinitionRefresh = true; + m_definitionRefreshCooldown = 0f; + } // Refresh streamed asset definitions when Motive actors or rigid bodies are added/removed. + if (m_pendingReplayDefinitionRefresh && m_definitionRefreshCooldown <= 0f && CanRefreshReplayDefinitions()) + { + m_pendingReplayDefinitionRefresh = false; + try + { + RefreshReplayDefinitionsForActiveTransport(); + m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f); + m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f); + } + catch (Exception ex) + { + m_definitionRefreshFailureCount++; + m_definitionRefreshCooldown = Mathf.Min(60f, 2f * Mathf.Pow(2f, Mathf.Min(m_definitionRefreshFailureCount, 5))); + m_pendingReplayDefinitionRefresh = true; + Debug.LogWarning(GetType().FullName + ": MMRP DataDescription refresh failed. Retrying in " + m_definitionRefreshCooldown + " seconds.", this); + Debug.LogException(ex, this); + } + } if (m_pendingDefinitionRefresh && m_definitionRefreshCooldown <= 0f && CanRefreshDefinitions()) { m_pendingDefinitionRefresh = false; @@ -782,6 +838,7 @@ public class OptitrackStreamingClient : MonoBehaviour { m_definitionRefreshFailureCount++; m_definitionRefreshCooldown = Mathf.Min(60f, 2f * Mathf.Pow(2f, Mathf.Min(m_definitionRefreshFailureCount, 5))); + m_pendingDefinitionRefresh = true; Debug.LogWarning(GetType().FullName + ": DataDescription refresh failed. Retrying in " + m_definitionRefreshCooldown + " seconds.", this); Debug.LogException(ex, this); } @@ -2024,8 +2081,14 @@ public class OptitrackStreamingClient : MonoBehaviour try { IPAddress replayGroup = IPAddress.Parse("239.255.42.100"); - if (!replayGroup.Equals(group)) - m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(replayGroup, local)); + IPAddress replayLocal = local; + if (!string.IsNullOrWhiteSpace(ReplayServerAddress)) + { + IPAddress replayServer = IPAddress.Parse(ReplayServerAddress.Trim()); + replayLocal = ResolveLocalAddress(replayServer); + } + if (!replayGroup.Equals(group) || !replayLocal.Equals(local)) + m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(replayGroup, replayLocal)); } catch (Exception ex) { @@ -2129,6 +2192,8 @@ public class OptitrackStreamingClient : MonoBehaviour if (!isReplayFrame && IsReplayFrameFresh()) return; + QueueDefinitionRefreshForFrameSource(isReplayFrame); + if (isReplayFrame) { m_replayReceivedFrameSinceConnect = true; @@ -2151,11 +2216,11 @@ public class OptitrackStreamingClient : MonoBehaviour if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // markerSets if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // unlabeled if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // rigidBodies - ParseDirectRigidBodies(data, rbCount, rbStart, rbEnd, frameTimestamp); + ParseDirectRigidBodies(data, rbCount, rbStart, rbEnd, frameTimestamp, isReplayFrame); int skelCount, skelStart, skelEnd; if (!ReadDirectSection(data, ref o, out skelCount, out skelStart, out skelEnd)) return; // skeletons - ParseDirectSkeletons(data, skelCount, skelStart, skelEnd, frameTimestamp); + ParseDirectSkeletons(data, skelCount, skelStart, skelEnd, frameTimestamp, isReplayFrame); int skipCount, skipStart, skipEnd; if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // assets @@ -2202,15 +2267,41 @@ public class OptitrackStreamingClient : MonoBehaviour return true; } - private void QueueStreamedTopologyDefinitionRefresh() + private void QueueStreamedTopologyDefinitionRefresh(bool preferReplayDefinitions) { if (!AutoRefreshDefinitions || SkipDataDescriptions) return; + if (preferReplayDefinitions && EnableReplayPriority && !string.IsNullOrWhiteSpace(ReplayServerAddress)) + { + if (m_pendingReplayDefinitionRefresh || m_forceReplayDefinitionRefreshSoon) + return; + + m_pendingReplayDefinitionRefresh = true; + m_forceReplayDefinitionRefreshSoon = true; + return; + } + + if (m_pendingDefinitionRefresh || m_forceDefinitionRefreshSoon) + return; + m_pendingDefinitionRefresh = true; m_forceDefinitionRefreshSoon = true; } + private void QueueDefinitionRefreshForFrameSource(bool isReplayFrame) + { + if (isReplayFrame) + { + if (m_directDefinitionSource != DirectDefinitionSource.Replay) + QueueStreamedTopologyDefinitionRefresh(true); + } + else if (m_directDefinitionSource == DirectDefinitionSource.Replay) + { + QueueStreamedTopologyDefinitionRefresh(false); + } + } + private static bool UpdateFrameTopologyIds(HashSet latestIds, HashSet currentIds, ref bool hasSnapshot) { if (!hasSnapshot) @@ -2231,7 +2322,7 @@ public class OptitrackStreamingClient : MonoBehaviour return true; } - private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp) + private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame) { int o = start; m_currentFrameRigidBodyIds.Clear(); @@ -2245,10 +2336,10 @@ public class OptitrackStreamingClient : MonoBehaviour } if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot)) - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); } - private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp) + private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame) { int o = start; m_currentFrameSkeletonIds.Clear(); @@ -2266,14 +2357,14 @@ public class OptitrackStreamingClient : MonoBehaviour if (!ReadDirectRigidBody(data, ref o, end, out stagedBones[b])) return; } - CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp); + CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp, isReplayFrame); } if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot)) - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); } - private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp) + private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame) { OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId); if (skelDef == null) @@ -2283,7 +2374,7 @@ public class OptitrackStreamingClient : MonoBehaviour if (skelDef == null) { - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); return; } @@ -2552,8 +2643,13 @@ public class OptitrackStreamingClient : MonoBehaviour if (hasServerInfo && DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket)) { - if (!SkipDataDescriptions && !DirectUpdateDefinitions(modelDefPacket)) - Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF parse failed. Falling back to synthetic skeleton definitions from incoming frames.", this); + if (!SkipDataDescriptions) + { + if (DirectUpdateDefinitions(modelDefPacket)) + m_directDefinitionSource = DirectDefinitionSource.Live; + else + Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF parse failed. Falling back to synthetic skeleton definitions from incoming frames.", this); + } } else if (hasServerInfo && !m_directNoModelDefWarned) { @@ -2629,6 +2725,7 @@ public class OptitrackStreamingClient : MonoBehaviour throw new InvalidOperationException("Direct NatNet MODELDEF request failed."); if (!DirectUpdateDefinitions(modelDefPacket)) throw new InvalidOperationException("Direct NatNet MODELDEF parse failed."); + m_directDefinitionSource = DirectDefinitionSource.Live; bool dataEndpointChanged = m_directServerAddress == null || @@ -2651,6 +2748,41 @@ public class OptitrackStreamingClient : MonoBehaviour ClientNatNetVersion = "Direct UDP"; } + private void UpdateDirectReplayDefinitions() + { + IPAddress serverAddr; + IPAddress localAddr; + UInt16 commandPort; + + try + { + serverAddr = IPAddress.Parse(ReplayServerAddress.Trim()); + commandPort = (UInt16)Mathf.Clamp(CommandPort, 1, 65535); + localAddr = ResolveLocalAddress(serverAddr); + } + catch (Exception ex) + { + throw new InvalidOperationException("Error parsing MMRP replay NatNet refresh settings.", ex); + } + + string hostName; + byte[] appVersion; + byte[] natNetVersion; + byte[] modelDefPacket; + UInt16 negotiatedDataPort; + IPAddress negotiatedMulticast; + + DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast); + + if (!DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket)) + throw new InvalidOperationException("MMRP replay NatNet MODELDEF request failed."); + if (!DirectUpdateDefinitions(modelDefPacket)) + throw new InvalidOperationException("MMRP replay NatNet MODELDEF parse failed."); + + m_directDefinitionSource = DirectDefinitionSource.Replay; + ClientNatNetVersion = "Direct UDP + MMRP"; + } + private bool DirectRequestServerInfo(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out string hostName, out byte[] appVersion, out byte[] natNetVersion, out UInt16 dataPort, out IPAddress multicastAddress) { hostName = ""; @@ -3138,11 +3270,6 @@ public class OptitrackStreamingClient : MonoBehaviour Debug.LogWarning( GetType().FullName + ": No frames received from the server yet. Verify your connection settings are correct and that the server is streaming.", this ); warnedPendingFirstFrame = true; - if ( AutoReconnect ) - { - Debug.Log( GetType().FullName + ": starting automatic reconnect while waiting for the first streaming frame.", this ); - Reconnect(); - } } continue; @@ -3251,7 +3378,7 @@ public class OptitrackStreamingClient : MonoBehaviour RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState); } if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot)) - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); // ---------------------- // - Update skeletons @@ -3284,7 +3411,7 @@ public class OptitrackStreamingClient : MonoBehaviour { Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this); } - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); continue; } @@ -3344,7 +3471,7 @@ public class OptitrackStreamingClient : MonoBehaviour skelState.DeliveryTimestamp = frameTimestamp; } if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot)) - QueueStreamedTopologyDefinitionRefresh(); + QueueStreamedTopologyDefinitionRefresh(isReplayFrame); // ----------------------------------------------------- // - Update trained markerset // trained markerset added