KNetworkTransform.cs

using UnityEngine;
using UnityEngine.Networking;
using System;
using System.Collections.Generic;

namespace AlderAcres.Network {
    /// <summary>
    /// 
    /// </summary>
    /// <remarks>
    /// Message structure is as follows:
    /// [ SyncVarDirtyBits (packed UInt32), TimeStamp (int), Payload (data depends on config) ]
    /// </remarks>
    public class KNetworkTransform : NetworkBehaviour {
        private const int RENDER_LATENCY = 100;// Render latency based on average ping in ms
        private const int MAX_EXTRAPOLATION_TIME = 500;// Max time in ms to extrapolate

        public delegate bool ValidateTransform3DCallback(ref Vector3 position, ref Quaternion rotation);
        public delegate bool ValidateRigidbody3DCallback(ref Vector3 position, ref Quaternion rotation, ref Vector3 velocity, ref Vector3 angularVelocity);

        enum SyncMode {
            None,
            Transform3D,
            Rigidbody3D,
        }

        enum InterpolationMode {
            None,
            Interpolate,
            InterpolateAndExtrapolate
        }

        enum AxisSyncMode {
            None,
            AxisX,
            AxisY,
            AxisZ,
            AxisXY,
            AxisXZ,
            AxisYZ,
            AxisXYZ
        }

        public ValidateTransform3DCallback ValidateTransform3D;
        public ValidateRigidbody3DCallback ValidateRigidbody3D;

        [Header("Sync Settings")]

        [Tooltip("Data to sync over the network")]
        [SerializeField]
        private SyncMode SyncMethod = SyncMode.Transform3D;

        [Tooltip("Number of messeges per second sent")]
        [Range(0, 30)]
        [SerializeField]
        private int SendRate = 10;

        [Tooltip("Interpolation method to use. None directly applies changes. Interpolate interpolates between the current position and the new position. Extrapolate uses dead rekoning to guess where the object will be if messages are missed. Both does interpolation if updates arrive when expected and extrapolates if update packet was missed")]
        [SerializeField]
        private InterpolationMode InterpolationMethod = InterpolationMode.Interpolate;

        [SerializeField]
        private float MovementThreshold = 0.001f;

        [Header("Transform Sync")]

        [SerializeField]
        private AxisSyncMode RotationSyncMethod = AxisSyncMode.AxisXYZ;

        [SerializeField]
        private bool CompressRotation = false;

        [Header("Rigidbody Sync")]

        [SerializeField]
        private bool SyncAngularVelocity = false;

        // State
        private Rigidbody Rigidbody;
        private NetworkWriter ClientWriter;
        private NetworkReader ServerReader;
        private NetworkState LastSyncedState = new NetworkState();
        private float SyncTimeDelta = 0f;

        private NetworkStateBuffer Buffer = new NetworkStateBuffer();

        // Cache vars
        private NetworkState state;

#pragma warning disable 0414
        private byte error;
#pragma warning restore 0414

        void Awake() {
            if (SyncMethod == SyncMode.Rigidbody3D) {
                Rigidbody = GetComponent<Rigidbody>();
                LastSyncedState.Set(transform, Rigidbody);
            }
            else {
                LastSyncedState.Set(transform);
            }

            if (localPlayerAuthority)
                ClientWriter = new NetworkWriter();
        }

        [ClientCallback]
        void Update() {
            if (!isClient)
                return;

            if (!hasAuthority)
                return;

            SyncTimeDelta += Time.deltaTime;

            // Sync if we reach the update interval
            if (SyncTimeDelta >= GetNetworkSendInterval()) {
                SyncToServer();

                SyncTimeDelta = 0f;
            }
        }

        void FixedUpdate() {
            if (isServer)
                FixedUpdateServer();

            if (isClient)
                FixedUpdateClient();
        }

        [Server]
        private void FixedUpdateServer() {
            if (syncVarDirtyBits != 0)
                return;

            if (!NetworkServer.active)
                return;

            if (!hasAuthority)
                return;

            if (!HasMoved())
                return;

            // Serialize from down to clients if moved on server
            SetDirtyBit(1);
        }

        [Client]
        private void FixedUpdateClient() {
            if (!NetworkServer.active && !NetworkClient.active)
                return;

            if (hasAuthority)
                return;

            if (Buffer.Count == 0)
                return;

            if (InterpolationMethod != InterpolationMode.None) {
                int delay = GetDelay(Buffer[0].Timestamp);

                // Interpolate if we can
                if (delay < RENDER_LATENCY) {
                    for (int i = 0; i < Buffer.Count; i++) {
                        NetworkState lastState = Buffer[i];
                        int lastStateDelay = GetDelay(lastState.Timestamp);
                        if (lastStateDelay >= RENDER_LATENCY || i == Buffer.Count - 1) {
                            NetworkState newestState = Buffer[Mathf.Max(i - 1, 0)];
                            int newestStateDelay = GetDelay(newestState.Timestamp);

                            float t = Mathf.InverseLerp(lastStateDelay, newestStateDelay, RENDER_LATENCY);

                            switch (SyncMethod) {
                                case SyncMode.None:
                                    break;
                                case SyncMode.Transform3D:
                                    Interpolate(lastState, newestState, t);
                                    break;
                                case SyncMode.Rigidbody3D:
                                    Interpolate(lastState, newestState, t);
                                    ApplyRigidbodyState(newestState);
                                    break;
                                default:
                                    break;
                            }

                            return;
                        }
                    }
                }
                // Extrapolate
                else {
                    if (InterpolationMethod == InterpolationMode.InterpolateAndExtrapolate) {
                        NetworkState olderState = Buffer[Mathf.Max(Buffer.Count - 1, 0)];
                        NetworkState newestState = Buffer[0];

                        if (newestState.Position != transform.position || newestState.Rotation != transform.rotation)
                            Extrapolate(olderState, newestState);

                        if (SyncMethod == SyncMode.Rigidbody3D)
                            ApplyRigidbodyState(newestState);

                        return;
                    }
                }
            }

            // Snap
            NetworkState newestState2 = Buffer[0];
            Snap(newestState2);

            if (SyncMethod == SyncMode.Rigidbody3D)
                ApplyRigidbodyState(newestState2);
        }

        private void Snap(NetworkState state) {
            transform.position = state.Position;

            if (RotationSyncMethod != AxisSyncMode.None)
                transform.rotation = state.Rotation;
        }

        private void Interpolate(NetworkState oldState, NetworkState newState, float t) {
            transform.position = Vector3.Lerp(oldState.Position, newState.Position, t);

            if (RotationSyncMethod != AxisSyncMode.None)
                transform.rotation = Quaternion.Lerp(oldState.Rotation, newState.Rotation, t);
        }

        private void Extrapolate(NetworkState previousState, NetworkState latestState) {
            if (SyncMethod == SyncMode.Rigidbody3D) {
                transform.position = Vector3.Lerp(previousState.Position, previousState.Position + latestState.Velocity, Time.fixedDeltaTime * latestState.Velocity.magnitude);

                if (RotationSyncMethod != AxisSyncMode.None) {
                    if (!SyncAngularVelocity) {
                        Vector3 angularVelocity = (Quaternion.Inverse(previousState.Rotation) * latestState.Rotation).eulerAngles / ((latestState.Timestamp - previousState.Timestamp) * 0.001f);
                        transform.rotation = Quaternion.Lerp(previousState.Rotation, previousState.Rotation * Quaternion.Euler(angularVelocity), Time.fixedDeltaTime * angularVelocity.magnitude);
                    }
                    else {
                        transform.rotation = Quaternion.Lerp(previousState.Rotation, previousState.Rotation * Quaternion.Euler(latestState.AngularVelocity), Time.fixedDeltaTime * latestState.AngularVelocity.magnitude);
                    }
                }
            }
        }

        private void ApplyRigidbodyState(NetworkState state) {
            Rigidbody.velocity = state.Velocity;

            if (SyncAngularVelocity)
                Rigidbody.angularVelocity = state.AngularVelocity;
        }

        private void ApplyServerCorrection(NetworkState correctedState) {
            // Get current position relative to the corrected state in the buffer
            int lastValidStateIndex = 0;
            for (int i = 0; i < Buffer.Count; i++) {
                if (Buffer[i].Timestamp > correctedState.Timestamp) {
                    lastValidStateIndex = i;
                    break;
                }
            }

            NetworkState lastValidState = Buffer[lastValidStateIndex];
            List<NetworkState> invalidStates = new List<NetworkState>();
            int startIndex = Mathf.Max(lastValidStateIndex - 1, 0);

            for (int i = startIndex; i >= 0; i--)
                invalidStates.Add(Buffer[i]);

            Buffer.DumpAfterState(lastValidState);

            NetworkState lastInvalidState = lastValidState;
            NetworkState lastState = lastValidState;
            for (int i = 0; i < invalidStates.Count; i++) {
                state = new NetworkState();
                state.Timestamp = invalidStates[i].Timestamp;
                state.Position = lastState.Position + (invalidStates[i].Position - lastInvalidState.Position);
                state.Rotation = lastState.Rotation * (lastInvalidState.Rotation * Quaternion.Inverse(invalidStates[i].Rotation));

                if (SyncMethod == SyncMode.Rigidbody3D) {
                    state.Velocity = lastState.Velocity + (invalidStates[i].Velocity - lastInvalidState.Velocity);

                    if (SyncAngularVelocity)
                        state.AngularVelocity = lastState.AngularVelocity + (invalidStates[i].AngularVelocity - lastInvalidState.AngularVelocity);
                }

                lastInvalidState = invalidStates[i];
                lastState = state;

                Buffer.Add(state);
            }

            // Apply latest state
            state = Buffer[0];
            Snap(state);

            if (SyncMethod == SyncMode.Rigidbody3D)
                ApplyRigidbodyState(state);
        }

        [Client]
        private void SyncToServer() {
            if (!HasMoved() || ClientScene.readyConnection == null)
                return;

            ClientWriter.SeekZero();

            ClientWriter.Write(NetworkTransport.GetNetworkTimestamp());

            switch (SyncMethod) {
                case SyncMode.Transform3D:
                    SerializeTransform3D(transform.position, transform.rotation, ClientWriter);
                    LastSyncedState.Set(transform);
                    break;
                case SyncMode.Rigidbody3D:
                    SerializeRigidbody3D(transform.position, transform.rotation, Rigidbody.velocity, Rigidbody.angularVelocity, ClientWriter);
                    LastSyncedState.Set(transform, Rigidbody);
                    break;
                default:
                    return;
            }

            ClientWriter.FinishMessage();
            CmdSendData(ClientWriter.ToArray());
        }

        [Client]
        public override void OnDeserialize(NetworkReader reader, bool initialState) {
            if (isServer && NetworkServer.localClientActive)
                return;

            if (!initialState) {
                uint dirtyBits = reader.ReadPackedUInt32();
                if (dirtyBits == 0)
                    return;

                // Server corrected state on our object
                if (dirtyBits == 2 && hasAuthority) {
                    NetworkState correctedState;
                    switch (SyncMethod) {
                        case SyncMode.Transform3D:
                            correctedState = DeserializeTransform3D(reader);
                            break;
                        case SyncMode.Rigidbody3D:
                            correctedState = DeserializeRigidbody3D(reader);
                            break;
                        default:
                            return;
                    }

                    ApplyServerCorrection(correctedState);
                    return;
                }

                // We own the object so ignore the update
                if (hasAuthority)
                    return;
            }

            switch (SyncMethod) {
                case SyncMode.Transform3D:
                    Buffer.Add(DeserializeTransform3D(reader));
                    break;
                case SyncMode.Rigidbody3D:
                    Buffer.Add(DeserializeRigidbody3D(reader));
                    break;
                default:
                    break;
            }
        }

        [Server]
        public override bool OnSerialize(NetworkWriter writer, bool initialState) {
            if (!initialState) {
                if (syncVarDirtyBits == 0) {
                    writer.WritePackedUInt32(0);
                    return false;
                }
                else {
                    writer.WritePackedUInt32(syncVarDirtyBits);
                }
            }

            if (hasAuthority) {
                switch (SyncMethod) {
                    case SyncMode.Transform3D:
                        SerializeTransform3D(transform.position, transform.rotation, writer);
                        break;
                    case SyncMode.Rigidbody3D:
                        SerializeRigidbody3D(transform.position, transform.rotation, Rigidbody.velocity, Rigidbody.angularVelocity, writer);
                        break;
                    default:
                        break;
                }
            }
            else {
                switch (SyncMethod) {
                    case SyncMode.Transform3D:
                        SerializeTransform3D(Buffer[0].Position, Buffer[0].Rotation, writer);
                        break;
                    case SyncMode.Rigidbody3D:
                        SerializeRigidbody3D(Buffer[0].Position, Buffer[0].Rotation, Buffer[0].Velocity, Buffer[0].AngularVelocity, writer);
                        break;
                    default:
                        break;
                }
            }

            return true;
        }

        [Server]
        private void OnDeserializeServer(NetworkReader reader) {
            // Ignore if we have authority over the object
            if (hasAuthority)
                return;

            reader.ReadBytes(4);// Reading unused data so it doesn't interfere

            switch (SyncMethod) {
                case SyncMode.Transform3D: {
                        state = DeserializeTransform3D(reader);

                        // Validate transform position
                        if (ValidateTransform3D != null) {
                            if (!ValidateTransform3D(ref state.Position, ref state.Rotation))
                                SetDirtyBit(2);// Set dirty so changes are sent back to clients next update
                        }

                        Buffer.Add(state);
                        break;
                    }
                case SyncMode.Rigidbody3D: {
                        state = DeserializeRigidbody3D(reader);

                        // Validate rigidbody data
                        if (ValidateRigidbody3D != null) {
                            if (!ValidateRigidbody3D(ref state.Position, ref state.Rotation, ref state.Velocity, ref state.AngularVelocity))
                                SetDirtyBit(2);// Set dirty so changes are sent back to clients next update
                        }

                        Buffer.Add(state);

                        break;
                    }
                default: {
                        return;
                    }
            }

            // Apply latest state recieved from client on server directly if no local client attached
            if (!isClient) {
                Snap(state);

                if (SyncMethod == SyncMode.Rigidbody3D)
                    ApplyRigidbodyState(state);
            }

            SetDirtyBit(1);// Set dirty to forward update to clients
        }

        private bool HasMoved() {
            // Check position
            if ((transform.position - LastSyncedState.Position).magnitude > MovementThreshold)
                return true;

            // Check rotation
            if (Quaternion.Angle(transform.rotation, LastSyncedState.Rotation) > MovementThreshold)
                return true;

            return false;
        }

        private int GetDelay(int timestamp) {
            if (isServer) {
                return NetworkTransport.GetRemoteDelayTimeMS(connectionToClient.hostId, connectionToClient.connectionId, timestamp, out error);
            }
            else {
                NetworkConnection conn = UnityEngine.Networking.NetworkManager.singleton.client.connection;
                return NetworkTransport.GetRemoteDelayTimeMS(conn.hostId, conn.connectionId, timestamp, out error);
            }
        }

        public override void OnStartAuthority() {
            SyncTimeDelta = 0;
        }

        public override int GetNetworkChannel() {
            return Channels.DefaultUnreliable;
        }

        public override float GetNetworkSendInterval() {
            return 1f / SendRate;
        }

        [Command]
        private void CmdSendData(byte[] data) {
            ServerReader = new NetworkReader(data);

            OnDeserializeServer(ServerReader);
        }

        // ---------- Serialization Helpers ----------

        private void SerializeTransform3D(Vector3 position, Quaternion rotation, NetworkWriter writer) {
            writer.Write(NetworkTransport.GetNetworkTimestamp());
            writer.Write(position);
            SerializationHelper.SerializeAxisVector3D(writer, rotation.eulerAngles, RotationSyncMethod, CompressRotation);
        }

        private void SerializeRigidbody3D(Vector3 position, Quaternion rotation, Vector3 velocity, Vector3 angularVelocity, NetworkWriter writer) {
            writer.Write(NetworkTransport.GetNetworkTimestamp());
            writer.Write(position);
            SerializationHelper.SerializeAxisVector3D(writer, rotation.eulerAngles, RotationSyncMethod, CompressRotation);
            writer.Write(velocity);

            if (SyncAngularVelocity)
                writer.Write(angularVelocity);
        }

        private NetworkState DeserializeTransform3D(NetworkReader reader) {
            NetworkState state = new NetworkState();

            state.Timestamp = reader.ReadInt32();
            state.Position = reader.ReadVector3();
            state.Rotation = Quaternion.Euler(SerializationHelper.DeserializeAxisVector3D(reader, RotationSyncMethod, CompressRotation));

            return state;
        }

        private NetworkState DeserializeRigidbody3D(NetworkReader reader) {
            NetworkState state = new NetworkState();

            state.Timestamp = reader.ReadInt32();
            state.Position = reader.ReadVector3();
            state.Rotation = Quaternion.Euler(SerializationHelper.DeserializeAxisVector3D(reader, RotationSyncMethod, CompressRotation));
            state.Velocity = reader.ReadVector3();

            if (SyncAngularVelocity)
                state.AngularVelocity = SerializationHelper.DeserializeAxisVector3D(reader, RotationSyncMethod, CompressRotation);

            return state;
        }

        private struct NetworkState {
            public int Timestamp;
            public Vector3 Position;
            public Quaternion Rotation;
            public Vector3 Velocity;
            public Vector3 AngularVelocity;

            public void Set(Transform transform) {
                Position = transform.position;
                Rotation = transform.rotation;
            }

            public void Set(Transform transform, Rigidbody rigidbody) {
                Position = transform.position;
                Rotation = transform.rotation;
                Velocity = rigidbody.velocity;
                AngularVelocity = rigidbody.angularVelocity;
            }
        }

        private class NetworkStateBuffer {
            private const int DEFAULT_BUFFER_SIZE = 20;

            public NetworkState this[int index]
            {
                get
                {
                    return BufferedState[index];
                }
            }
            private NetworkState[] BufferedState;

            public int Count
            {
                get
                {
                    return TimestampCount;
                }
            }
            private int TimestampCount = 0;

            public NetworkStateBuffer(int bufferSize = 0) {
                if (bufferSize > DEFAULT_BUFFER_SIZE)
                    BufferedState = new NetworkState[bufferSize];
                else
                    BufferedState = new NetworkState[DEFAULT_BUFFER_SIZE];
            }

            public void Add(NetworkState newState) {
                ShiftBuffer();

                BufferedState[0] = newState;
                TimestampCount = Math.Min(BufferedState.Length, TimestampCount + 1);

#if UNITY_EDITOR || DEBUG
                IntegrityCheckBuffer();
#endif
            }

            public void DumpAfterState(NetworkState state) {
                int index = 0;
                for (int i = BufferedState.Length - 1; i >= 1; i++) {
                    if (BufferedState[i].Timestamp == state.Timestamp) {
                        index = i;
                        break;
                    }
                }

                for (int i = index; i >= 1; i--) {
                    BufferedState[i] = default(NetworkState);
                    TimestampCount -= 1;
                }
            }

            private void ShiftBuffer() {
                for (int i = BufferedState.Length - 1; i >= 1; i--) {
                    BufferedState[i] = BufferedState[i - 1];
                }
            }

            private void IntegrityCheckBuffer() {
                for (int i = 0; i < TimestampCount - 1; i++) {
                    if (BufferedState[i].Timestamp < BufferedState[i + 1].Timestamp)
                        LogFilter.Log("State buffer inconsistency found!", this, LogLevel.Warning);
                }
            }
        }

        private static class SerializationHelper {
            public static void SerializeAxisVector3D(NetworkWriter writer, Vector3 vector, AxisSyncMode mode, bool compress) {
                switch (mode) {
                    case AxisSyncMode.None:
                        break;
                    case AxisSyncMode.AxisX:
                        WriteAngle(writer, vector.x, compress);
                        break;
                    case AxisSyncMode.AxisY:
                        WriteAngle(writer, vector.y, compress);
                        break;
                    case AxisSyncMode.AxisZ:
                        WriteAngle(writer, vector.z, compress);
                        break;
                    case AxisSyncMode.AxisXY:
                        WriteAngle(writer, vector.x, compress);
                        WriteAngle(writer, vector.y, compress);
                        break;
                    case AxisSyncMode.AxisXZ:
                        WriteAngle(writer, vector.x, compress);
                        WriteAngle(writer, vector.z, compress);
                        break;
                    case AxisSyncMode.AxisYZ:
                        WriteAngle(writer, vector.y, compress);
                        WriteAngle(writer, vector.z, compress);
                        break;
                    case AxisSyncMode.AxisXYZ:
                        WriteAngle(writer, vector.x, compress);
                        WriteAngle(writer, vector.y, compress);
                        WriteAngle(writer, vector.z, compress);
                        break;
                }
            }

            public static Vector3 DeserializeAxisVector3D(NetworkReader reader, AxisSyncMode mode, bool compressed) {
                Vector3 axisVector = Vector3.zero;

                switch (mode) {
                    case AxisSyncMode.None:
                        break;
                    case AxisSyncMode.AxisX:
                        axisVector.Set(ReadAngle(reader, compressed), 0, 0);
                        break;
                    case AxisSyncMode.AxisY:
                        axisVector.Set(0, ReadAngle(reader, compressed), 0);
                        break;
                    case AxisSyncMode.AxisZ:
                        axisVector.Set(0, 0, ReadAngle(reader, compressed));
                        break;
                    case AxisSyncMode.AxisXY:
                        axisVector.Set(ReadAngle(reader, compressed), ReadAngle(reader, compressed), 0);
                        break;
                    case AxisSyncMode.AxisXZ:
                        axisVector.Set(ReadAngle(reader, compressed), 0, ReadAngle(reader, compressed));
                        break;
                    case AxisSyncMode.AxisYZ:
                        axisVector.Set(0, ReadAngle(reader, compressed), ReadAngle(reader, compressed));
                        break;
                    case AxisSyncMode.AxisXYZ:
                        axisVector.Set(ReadAngle(reader, compressed), ReadAngle(reader, compressed), ReadAngle(reader, compressed));
                        break;
                }

                return axisVector;
            }

            public static void WriteAngle(NetworkWriter writer, float angle, bool compress) {
                if (!compress)
                    writer.Write(angle);
                else
                    writer.Write((short)angle);
            }

            public static float ReadAngle(NetworkReader reader, bool compress) {
                if (!compress)
                    return reader.ReadSingle();
                else
                    return reader.ReadInt16();
            }
        }
    }
}