LeapServiceProvider.cs

/******************************************************************************
 * Copyright (C) Leap Motion, Inc. 2011-2018.                                 *
 * Leap Motion proprietary and confidential.                                  *
 *                                                                            *
 * Use subject to the terms of the Leap Motion SDK Agreement available at     *
 * https://developer.leapmotion.com/sdk_agreement, or another agreement       *
 * between Leap Motion and you, your company or other organization.           *
 ******************************************************************************/

/* Modified
 * by: Zacryon
 * Date: 23.06.2019
 * Notes: Added a delay functionality. Modifications not marked.
 */

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

namespace Leap.Unity
{
    using Attributes;

    /// <summary>
    /// The LeapServiceProvider provides tracked Leap Hand data and images from the device
    /// via the Leap service running on the client machine.
    /// </summary>
    public class LeapServiceProvider : LeapProvider
    {
        #region Delay parameters and additional events
        [Tooltip("Starts the hand tracking in delay mode if enabled.")]
        public bool enableDelay = false;
        [Tooltip("Delay is not supported for interpolation mode yet.\n" +
                 "If you use the interpolation feature the delay settings will be ignored.")]
        public bool _useInterpolation = false;
        [Tooltip("Delay duration in milliseconds. Supports float point values as well.")]
        public double delayInMilliseconds = 350;
        #endregion

        #region Constants

        /// <summary>
        /// Converts nanoseconds to seconds.
        /// </summary>
        protected const double NS_TO_S = 1e-6;

        /// <summary>
        /// Converts seconds to nanoseconds.
        /// </summary>
        protected const double S_TO_NS = 1e6;

        /// <summary>
        /// The transform array used for late-latching.
        /// </summary>
        protected const string HAND_ARRAY_GLOBAL_NAME = "_LeapHandTransforms";

        /// <summary>
        /// The maximum number of times the provider will
        /// attempt to reconnect to the service before giving up.
        /// </summary>
        protected const int MAX_RECONNECTION_ATTEMPTS = 5;

        /// <summary>
        /// The number of frames to wait between each
        /// reconnection attempt.
        /// </summary>
        protected const int RECONNECTION_INTERVAL = 180;

        #endregion

        #region Inspector

        public enum FrameOptimizationMode
        {
            None,
            ReuseUpdateForPhysics,
            ReusePhysicsForUpdate,
        }
        [Tooltip("When enabled, the provider will only calculate one leap frame instead of two.")]
        [SerializeField]
        protected FrameOptimizationMode _frameOptimization = FrameOptimizationMode.None;

        public enum PhysicsExtrapolationMode
        {
            None,
            Auto,
            Manual
        }
        [Tooltip("The mode to use when extrapolating physics.\n" +
                 " None - No extrapolation is used at all.\n" +
                 " Auto - Extrapolation is chosen based on the fixed timestep.\n" +
                 " Manual - Extrapolation time is chosen manually by the user.")]
        [SerializeField]
        protected PhysicsExtrapolationMode _physicsExtrapolation = PhysicsExtrapolationMode.Auto;

        [Tooltip("The amount of time (in seconds) to extrapolate the physics data by.")]
        [SerializeField]
        protected float _physicsExtrapolationTime = 1.0f / 90.0f;

#if UNITY_2017_3_OR_NEWER
        [Tooltip("When checked, profiling data from the LeapCSharp worker thread will be used to populate the UnityProfiler.")]
        [EditTimeOnly]
#else
    [Tooltip("Worker thread profiling requires a Unity version of 2017.3 or greater.")]
    [Disable]
#endif
        [SerializeField]
        protected bool _workerThreadProfiling = false;

        #endregion

        #region Internal Settings & Memory

        // Extrapolate on Android to compensate for the latency introduced by its graphics
        // pipeline.
#if UNITY_ANDROID && !UNITY_EDITOR
    protected int ExtrapolationAmount = 15;
    protected int BounceAmount = 70;
#else
        protected int ExtrapolationAmount = 0;
        protected int BounceAmount = 0;
#endif

        protected Controller _leapController;
        protected bool _isDestroyed;

        protected SmoothedFloat _fixedOffset = new SmoothedFloat();
        protected SmoothedFloat _smoothedTrackingLatency = new SmoothedFloat();
        protected long _unityToLeapOffset;

        protected Frame _untransformedUpdateFrame;
        protected Frame _transformedUpdateFrame;
        protected Frame _untransformedFixedFrame;
        protected Frame _transformedFixedFrame;

        #endregion

        #region Edit-time Frame Data

        private Action<Device> _onDeviceSafe;
        /// <summary>
        /// A utility event to get a callback whenever a new device is connected to the service.
        /// This callback will ALSO trigger a callback upon subscription if a device is already
        /// connected.
        ///
        /// For situations with multiple devices OnDeviceSafe will be dispatched once for each device.
        /// </summary>
        public event Action<Device> OnDeviceSafe
        {
            add
            {
                if (_leapController != null && _leapController.IsConnected)
                {
                    foreach (var device in _leapController.Devices)
                    {
                        value(device);
                    }
                }
                _onDeviceSafe += value;
            }
            remove
            {
                _onDeviceSafe -= value;
            }
        }

#if UNITY_EDITOR
        private Frame _backingUntransformedEditTimeFrame = null;
        private Frame _untransformedEditTimeFrame
        {
            get
            {
                if (_backingUntransformedEditTimeFrame == null)
                {
                    _backingUntransformedEditTimeFrame = new Frame();
                }
                return _backingUntransformedEditTimeFrame;
            }
        }
        private Frame _backingEditTimeFrame = null;
        private Frame _editTimeFrame
        {
            get
            {
                if (_backingEditTimeFrame == null)
                {
                    _backingEditTimeFrame = new Frame();
                }
                return _backingEditTimeFrame;
            }
        }

        private Dictionary<TestHandFactory.TestHandPose, Hand> _cachedLeftHands
          = new Dictionary<TestHandFactory.TestHandPose, Hand>();
        private Hand _editTimeLeftHand
        {
            get
            {
                Hand cachedHand;
                if (_cachedLeftHands.TryGetValue(editTimePose, out cachedHand))
                {
                    return cachedHand;
                }
                else
                {
                    cachedHand = TestHandFactory.MakeTestHand(isLeft: true, pose: editTimePose);
                    _cachedLeftHands[editTimePose] = cachedHand;
                    return cachedHand;
                }
            }
        }

        private Dictionary<TestHandFactory.TestHandPose, Hand> _cachedRightHands
          = new Dictionary<TestHandFactory.TestHandPose, Hand>();
        private Hand _editTimeRightHand
        {
            get
            {
                Hand cachedHand;
                if (_cachedRightHands.TryGetValue(editTimePose, out cachedHand))
                {
                    return cachedHand;
                }
                else
                {
                    cachedHand = TestHandFactory.MakeTestHand(isLeft: false, pose: editTimePose);
                    _cachedRightHands[editTimePose] = cachedHand;
                    return cachedHand;
                }
            }
        }

#endif

        #endregion

        #region LeapProvider Implementation

        public override Frame CurrentFrame
        {
            get
            {
#if UNITY_EDITOR
                if (!Application.isPlaying)
                {
                    _editTimeFrame.Hands.Clear();
                    _untransformedEditTimeFrame.Hands.Clear();
                    _untransformedEditTimeFrame.Hands.Add(_editTimeLeftHand);
                    _untransformedEditTimeFrame.Hands.Add(_editTimeRightHand);
                    transformFrame(_untransformedEditTimeFrame, _editTimeFrame);
                    return _editTimeFrame;
                }
#endif
                if (_frameOptimization == FrameOptimizationMode.ReusePhysicsForUpdate)
                {
                    return _transformedFixedFrame;
                }
                else
                {
                    return _transformedUpdateFrame;
                }
            }
        }

        public override Frame CurrentFixedFrame
        {
            get
            {
#if UNITY_EDITOR
                if (!Application.isPlaying)
                {
                    _editTimeFrame.Hands.Clear();
                    _untransformedEditTimeFrame.Hands.Clear();
                    _untransformedEditTimeFrame.Hands.Add(_editTimeLeftHand);
                    _untransformedEditTimeFrame.Hands.Add(_editTimeRightHand);
                    transformFrame(_untransformedEditTimeFrame, _editTimeFrame);
                    return _editTimeFrame;
                }
#endif
                if (_frameOptimization == FrameOptimizationMode.ReuseUpdateForPhysics)
                {
                    return _transformedUpdateFrame;
                }
                else
                {
                    return _transformedFixedFrame;
                }
            }
        }

        #endregion

        #region Unity Events

        protected virtual void Reset()
        {
            editTimePose = TestHandFactory.TestHandPose.DesktopModeA;
        }

        protected virtual void Awake()
        {
            _fixedOffset.delay = 0.4f;
            _smoothedTrackingLatency.SetBlend(0.99f, 0.0111f);
        }

        protected virtual void Start()
        {
            createController();
            _transformedUpdateFrame = new Frame();
            _transformedFixedFrame = new Frame();
            _untransformedUpdateFrame = new Frame();
            _untransformedFixedFrame = new Frame();
        }

        protected virtual void Update()
        {
            if (_workerThreadProfiling)
            {
                LeapProfiling.Update();
            }

            if (!checkConnectionIntegrity()) { return; }

#if UNITY_EDITOR
            if (UnityEditor.EditorApplication.isCompiling)
            {
                UnityEditor.EditorApplication.isPlaying = false;
                Debug.LogWarning("Unity hot reloading not currently supported. Stopping Editor Playback.");
                return;
            }
#endif

            _fixedOffset.Update(Time.time - Time.fixedTime, Time.deltaTime);

            if (_frameOptimization == FrameOptimizationMode.ReusePhysicsForUpdate)
            {
                DispatchUpdateFrameEvent(_transformedFixedFrame);
                return;
            }

            if (_useInterpolation)
            {
#if !UNITY_ANDROID || UNITY_EDITOR
                _smoothedTrackingLatency.value = Mathf.Min(_smoothedTrackingLatency.value, 30000f);
                _smoothedTrackingLatency.Update((float)(_leapController.Now() - _leapController.FrameTimestamp()), Time.deltaTime);
#endif
                long timestamp = CalculateInterpolationTime() + (ExtrapolationAmount * 1000);
                _unityToLeapOffset = timestamp - (long)(Time.time * S_TO_NS);

                _leapController.GetInterpolatedFrameFromTime(_untransformedUpdateFrame, timestamp, CalculateInterpolationTime() - (BounceAmount * 1000));
            }
            else
            {
                _leapController.Frame(_untransformedUpdateFrame);
            }

            if (_untransformedUpdateFrame != null)
            {
                transformFrame(_untransformedUpdateFrame, _transformedUpdateFrame);

                DispatchUpdateFrameEvent(_transformedUpdateFrame);
            }
        }

        protected virtual void FixedUpdate()
        {
            if (_frameOptimization == FrameOptimizationMode.ReuseUpdateForPhysics)
            {
                DispatchFixedFrameEvent(_transformedUpdateFrame);
                return;
            }

            if (_useInterpolation)
            {

                long timestamp;
                switch (_frameOptimization)
                {
                    case FrameOptimizationMode.None:
                        // By default we use Time.fixedTime to ensure that our hands are on the same
                        // timeline as Update.  We add an extrapolation value to help compensate
                        // for latency.
                        float extrapolatedTime = Time.fixedTime + CalculatePhysicsExtrapolation();
                        timestamp = (long)(extrapolatedTime * S_TO_NS) + _unityToLeapOffset;
                        break;
                    case FrameOptimizationMode.ReusePhysicsForUpdate:
                        // If we are re-using physics frames for update, we don't even want to care
                        // about Time.fixedTime, just grab the most recent interpolated timestamp
                        // like we are in Update.
                        timestamp = CalculateInterpolationTime() + (ExtrapolationAmount * 1000);
                        break;
                    default:
                        throw new System.InvalidOperationException(
                          "Unexpected frame optimization mode: " + _frameOptimization);
                }
                _leapController.GetInterpolatedFrame(_untransformedFixedFrame, timestamp);

            }
            else
            {
                _leapController.Frame(_untransformedFixedFrame);
            }

            if (_untransformedFixedFrame != null)
            {
                transformFrame(_untransformedFixedFrame, _transformedFixedFrame);

                DispatchFixedFrameEvent(_transformedFixedFrame);
            }
        }

        protected virtual void OnDestroy()
        {
            destroyController();
            _isDestroyed = true;
        }

        protected virtual void OnApplicationPause(bool isPaused)
        {
            if (_leapController != null)
            {
                if (isPaused)
                {
                    _leapController.StopConnection();
                }
                else
                {
                    _leapController.StartConnection();
                }
            }
        }

        protected virtual void OnApplicationQuit()
        {
            destroyController();
            _isDestroyed = true;
        }

        public float CalculatePhysicsExtrapolation()
        {
            switch (_physicsExtrapolation)
            {
                case PhysicsExtrapolationMode.None:
                    return 0;
                case PhysicsExtrapolationMode.Auto:
                    return Time.fixedDeltaTime;
                case PhysicsExtrapolationMode.Manual:
                    return _physicsExtrapolationTime;
                default:
                    throw new System.InvalidOperationException(
                      "Unexpected physics extrapolation mode: " + _physicsExtrapolation);
            }
        }

        #endregion

        #region Public API

        /// <summary>
        /// Returns the Leap Controller instance.
        /// </summary>
        public Controller GetLeapController()
        {
#if UNITY_EDITOR
            // Null check to deal with hot reloading.
            if (!_isDestroyed && _leapController == null)
            {
                createController();
            }
#endif
            return _leapController;
        }

        /// <summary>
        /// Returns true if the Leap Motion hardware is plugged in and this application is
        /// connected to the Leap Motion service.
        /// </summary>
        public bool IsConnected()
        {
            return GetLeapController().IsConnected;
        }

        /// <summary>
        /// Retransforms hand data from Leap space to the space of the Unity transform.
        /// This is only necessary if you're moving the LeapServiceProvider around in a
        /// custom script and trying to access Hand data from it directly afterward.
        /// </summary>
        public void RetransformFrames()
        {
            transformFrame(_untransformedUpdateFrame, _transformedUpdateFrame);
            transformFrame(_untransformedFixedFrame, _transformedFixedFrame);
        }

        /// <summary>
        /// Copies property settings from this LeapServiceProvider to the target
        /// LeapXRServiceProvider where applicable. Does not modify any XR-specific settings
        /// that only exist on the LeapXRServiceProvider.
        /// </summary>
        public void CopySettingsToLeapXRServiceProvider(
            LeapXRServiceProvider leapXRServiceProvider)
        {
            leapXRServiceProvider._frameOptimization = _frameOptimization;
            leapXRServiceProvider._physicsExtrapolation = _physicsExtrapolation;
            leapXRServiceProvider._physicsExtrapolationTime = _physicsExtrapolationTime;
            leapXRServiceProvider._workerThreadProfiling = _workerThreadProfiling;
        }

        #region Delay event handlers
        /// <summary>
        /// Event handler, which enables the delay mode. Does not use any arguments.
        /// </summary>
        public void EnableDelayHandler(System.Object sender, EventArgs evtArgs)
        {
            if (_leapController == null)
                createController();

            enableDelay = true;
            _leapController.SetDelayActive(enableDelay);
        }
        /// <summary>
        /// Event handler, which disables the delay mode. Does not use any arguments.
        /// </summary>
        public void DisableDelayHandler(System.Object sender, EventArgs evtArgs)
        {
            enableDelay = false;
            if (_leapController != null)
                _leapController.SetDelayActive(enableDelay);
        }
        #endregion

        #endregion

        #region Internal Methods

        protected virtual long CalculateInterpolationTime(bool endOfFrame = false)
        {
#if UNITY_ANDROID && !UNITY_EDITOR
      return _leapController.Now() - 16000;
#else
            if (_leapController != null)
            {
                return _leapController.Now() - (long)_smoothedTrackingLatency.value;
            }
            else
            {
                return 0;
            }
#endif
        }

        /// <summary>
        /// Initializes Leap Motion policy flags.
        /// </summary>
        protected virtual void initializeFlags()
        {
            if (_leapController == null)
            {
                return;
            }

            _leapController.ClearPolicy(Controller.PolicyFlag.POLICY_DEFAULT);
        }

        /// <summary>
        /// Creates an instance of a Controller, initializing its policy flags and
        /// subscribing to its connection event.
        /// Also sets the delay mode and value if provided.
        /// Triggers an OnControllerCreated event as soon as the controller was instantiated.
        /// </summary>
        protected void createController()
        {
            if (_leapController != null)
            {
                return;
            }

            _leapController = new Controller(enableDelay, delayInMilliseconds);
            _leapController.Device += (s, e) => {
                if (_onDeviceSafe != null)
                {
                    _onDeviceSafe(e.Device);
                }
            };

            if (_leapController.IsConnected)
            {
                initializeFlags();
            }
            else
            {
                _leapController.Device += onHandControllerConnect;
            }

            if (_workerThreadProfiling)
            {
                //A controller will report profiling statistics for the duration of it's lifetime
                //so these events will never be unsubscribed from.
                _leapController.EndProfilingBlock += LeapProfiling.EndProfilingBlock;
                _leapController.BeginProfilingBlock += LeapProfiling.BeginProfilingBlock;

                _leapController.EndProfilingForThread += LeapProfiling.EndProfilingForThread;
                _leapController.BeginProfilingForThread += LeapProfiling.BeginProfilingForThread;
            }
        }

        /// <summary>
        /// Stops the connection for the existing instance of a Controller, clearing old
        /// policy flags and resetting the Controller to null.
        /// Triggers an OnControllerDestroyed shortly before the controller is completely destroyed.
        /// </summary>
        protected void destroyController()
        {
            if (_leapController != null)
            {
                if (_leapController.IsConnected)
                {
                    _leapController.ClearPolicy(Controller.PolicyFlag.POLICY_OPTIMIZE_HMD);
                }
                _leapController.StopConnection();
                _leapController = null;
            }
        }

        private int _framesSinceServiceConnectionChecked = 0;
        private int _numberOfReconnectionAttempts = 0;
        /// <summary>
        /// Checks whether this provider is connected to a service;
        /// If it is not, attempt to reconnect at regular intervals
        /// for MAX_RECONNECTION_ATTEMPTS
        /// </summary>
        protected bool checkConnectionIntegrity()
        {
            if (_leapController.IsServiceConnected)
            {
                _framesSinceServiceConnectionChecked = 0;
                _numberOfReconnectionAttempts = 0;
                return true;
            }
            else if (_numberOfReconnectionAttempts < MAX_RECONNECTION_ATTEMPTS)
            {
                _framesSinceServiceConnectionChecked++;

                if (_framesSinceServiceConnectionChecked > RECONNECTION_INTERVAL)
                {
                    _framesSinceServiceConnectionChecked = 0;
                    _numberOfReconnectionAttempts++;

                    Debug.LogWarning("Leap Service not connected; attempting to reconnect for try " +
                                     _numberOfReconnectionAttempts + "/" + MAX_RECONNECTION_ATTEMPTS +
                                     "...", this);
                    using (new ProfilerSample("Reconnection Attempt"))
                    {
                        destroyController();
                        createController();
                    }
                }
            }
            return false;
        }

        protected void onHandControllerConnect(object sender, LeapEventArgs args)
        {
            initializeFlags();

            if (_leapController != null)
            {
                _leapController.Device -= onHandControllerConnect;
            }
        }

        protected virtual void transformFrame(Frame source, Frame dest)
        {
            dest.CopyFrom(source).Transform(transform.GetLeapMatrix());
        }

        #endregion

    }

}