text: /******************************************************************************
   * Copyright (C) Ultraleap, Inc. 2011-2020.                                   *
   *                                                                            *
   * Use subject to the terms of the Apache License 2.0 available at            *
   * http://www.apache.org/licenses/LICENSE-2.0, or another agreement           *
   * between Ultraleap and you, your company or other organization.             *
   ******************************************************************************/
  
  using InteractionEngineUtility;
  using System.Collections;
  using System.Collections.Generic;
  using UnityEngine;
  
  namespace Leap.Unity.Interaction {
  
    /// <summary>
    /// The sliding window throw handler implements a simple heuristic that provides a
    /// reasonably accurate measure of the user's intended "throw direction" for a physical
    /// object. It is used as the default implementation of an Interaction Behaviour's
    /// throw handler.
    /// </summary>
    public class SlidingWindowThrow : IThrowHandler {
  
      /// <summary>
      /// The length of the averaging window in seconds.
      /// </summary>
      private float _windowLength = 0.05F;
  
      /// <summary>
      /// The delay between the averaging window and the current time.
      /// </summary>
      private float _windowDelay = 0.02F;
  
      /// <summary>
      /// A curve that maps the speed of the object upon release to a multiplier to apply
      /// to that speed as the throw occurs.
      /// </summary>
      private AnimationCurve _velocityMultiplierCurve = new AnimationCurve(
                                                          new Keyframe(0.0F, 1.0F, 0, 0),
                                                          new Keyframe(3.0F, 1.5F, 0, 0));
  
      private struct VelocitySample {
        public float time;
        public Vector3 position;
        public Quaternion rotation;
  
        public VelocitySample(Vector3 position, Quaternion rotation, float time) {
          this.position = position;
          this.rotation = rotation;
          this.time = Time.fixedTime;
        }
  
        public static VelocitySample Interpolate(VelocitySample a, VelocitySample b, float time) {
          float alpha = Mathf.Clamp01(Mathf.InverseLerp(a.time, b.time, time));
  
          return new VelocitySample(Vector3.Lerp(a.position, b.position, alpha),
                                    Quaternion.Slerp(a.rotation, b.rotation, alpha),
                                    time);
        }
      }
  
      private Queue<VelocitySample> _velocityQueue = new Queue<VelocitySample>(64);
  
      /// <summary>
      /// Samples the current velocity and adds it to a rolling average.
      /// </summary>
      public void OnHold(InteractionBehaviour intObj,
                         ReadonlyList<InteractionController> controllers) {
        _velocityQueue.Enqueue(new VelocitySample(intObj.rigidbody.position,
                                                  intObj.rigidbody.rotation,
                                                  Time.fixedTime));
  
        while (true) {
          VelocitySample oldestVelocity = _velocityQueue.Peek();
  
          // Dequeue conservatively if the oldest velocity is more than 4 frames later
          // than the start of the window.
          if (oldestVelocity.time + (Time.fixedDeltaTime * 4) < Time.fixedTime
                                                                - _windowLength
                                                                - _windowDelay) {
            _velocityQueue.Dequeue();
          }
          else {
            break;
          }
        }
      }
  
      /// <summary>
      /// Transfers the averaged velocity to the released object.
      /// </summary>
      public void OnThrow(InteractionBehaviour intObj, InteractionController throwingController) {
        if (_velocityQueue.Count < 2) {
          intObj.rigidbody.velocity = Vector3.zero;
          intObj.rigidbody.angularVelocity = Vector3.zero;
          return;
        }
  
        float windowEnd = Time.fixedTime - _windowDelay;
        float windowStart = windowEnd - _windowLength;
  
        // 0 occurs before 1,
        // start occurs before end.
        VelocitySample start0, start1;
        VelocitySample end0, end1;
        VelocitySample s0, s1;
  
        s0 = s1 = start0 = start1 = end0 = end1 = _velocityQueue.Dequeue();
  
        while (_velocityQueue.Count != 0) {
          s0 = s1;
          s1 = _velocityQueue.Dequeue();
  
          if (s0.time < windowStart && s1.time >= windowStart) {
            start0 = s0;
            start1 = s1;
          }
  
          if (s0.time < windowEnd && s1.time >= windowEnd) {
            end0 = s0;
            end1 = s1;
  
            // We have assigned both start and end and can break out of the loop.
            _velocityQueue.Clear();
            break;
          }
        }
  
        VelocitySample start = VelocitySample.Interpolate(start0, start1, windowStart);
        VelocitySample end = VelocitySample.Interpolate(end0, end1, windowEnd);
  
        Vector3 interpolatedVelocity = PhysicsUtility.ToLinearVelocity(start.position,
                                                                       end.position,
                                                                       _windowLength);
  
        intObj.rigidbody.velocity = interpolatedVelocity;
        intObj.rigidbody.angularVelocity = PhysicsUtility.ToAngularVelocity(start.rotation,
                                                                            end.rotation,
                                                                            _windowLength);
  
        intObj.rigidbody.velocity *= _velocityMultiplierCurve.Evaluate(intObj.rigidbody.velocity.magnitude);
      }
  
    }
  
  }
  