Fix MMRP replay definition refresh

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))
+                    IPAddress replayLocal = local;
-                        m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(replayGroup, 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<Int32> latestIds, HashSet<Int32> 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))
+            if (!SkipDataDescriptions)
-                Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF parse failed. Falling back to synthetic skeleton definitions from incoming frames.", this);
+            {
+                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