AppExperiments

/******************************************************************************
 * 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.           *
 ******************************************************************************/

using Leap.Unity.Query;
using UnityEngine;

namespace Leap.Unity {

  /// <summary>
  /// Allows you to add to a capped-size ring buffer of Ts and, when full, compute the
  /// buffer's average change over time. DeltaBuffer without type parameters supports
  /// Vector3s; DeltaFloatBuffer supports floats, and DeltaQuaternionBuffer supports
  /// Quaternion rotations.
  /// 
  /// To support other types, subclass DeltaBuffer with your sample type and average
  /// change type (in many cases the these are the same) and implement the Delta()
  /// function to compute the average change of samples currently in the buffer.
  /// </summary>
  public abstract class DeltaBuffer<SampleType, DerivativeType> : IIndexable<SampleType> {

    protected struct ValueTimePair {
      public SampleType value;
      public float time;
    }

    public DeltaBuffer(int bufferSize) {
      _buffer = new RingBuffer<ValueTimePair>(bufferSize);
    }

    protected RingBuffer<ValueTimePair> _buffer;

    public int Count { get { return _buffer.Count; } }

    public bool IsFull { get { return _buffer.IsFull; } }

    public bool IsEmpty { get { return _buffer.IsEmpty; } }

    public int Capacity { get { return _buffer.Capacity; } }

    public SampleType this[int idx] {
      get { return _buffer[idx].value; }
    }

    public void Clear() { _buffer.Clear(); }

    public void Add(SampleType sample, float sampleTime) {
      if (!IsEmpty && sampleTime == GetLatestTime()) {
        SetLatest(sample, sampleTime);
        return;
      }

      _buffer.Add(new ValueTimePair { value = sample, time = sampleTime });
    }

    public SampleType Get(int idx) {
      return _buffer.Get(idx).value;
    }

    public SampleType GetLatest() {
      return Get(Count - 1);
    }

    public void Set(int idx, SampleType sample, float sampleTime) {
      _buffer.Set(idx, new ValueTimePair { value = sample, time = sampleTime });
    }

    public void SetLatest(SampleType sample, float sampleTime) {
      if (Count == 0) Set(0, sample, sampleTime);
      else Set(Count - 1, sample, sampleTime);
    }

    public float GetTime(int idx) {
      return _buffer.Get(idx).time;
    }

    public float GetLatestTime() {
      return _buffer.Get(Count - 1).time;
    }

    /// <summary>
    /// Returns the average change between each sample per unit time.
    /// 
    /// If the buffer is empty, you should return the identity for your derivative type.
    /// </summary>
    public abstract DerivativeType Delta();

    #region foreach Support

    public IndexableEnumerator<SampleType> GetEnumerator() {
      return new IndexableEnumerator<SampleType>(this);
    }

    #endregion

  }

  /// <summary>
  /// A ring buffer of Vector3s with a Delta() function that computes the buffer's
  /// average change over time.
  /// 
  /// The larger the buffer, the more stable but also delayed the resulting average
  /// change over time. A buffer size of 5 is a good start for 60-90 Hz updates.
  /// </summary>
  public class DeltaBuffer : DeltaBuffer<Vector3, Vector3> {

    public DeltaBuffer(int bufferSize) : base(bufferSize) { }

    /// <summary>
    /// Returns the average change between each sample per unit time, or zero if the
    /// buffer contains one or fewer elements.
    /// 
    /// The larger the buffer, the more stable but also delayed the resulting average
    /// change over time. A buffer size of 5 is a good start for 60-90 Hz updates.
    /// </summary>
    public override Vector3 Delta() {
      if (Count <= 1) { return Vector3.zero; }

      Vector3 deltaPerTimeSum = Vector3.zero;
      for (int i = 0; i < Count - 1; i++) {
        deltaPerTimeSum += (Get(i + 1) - Get(i)) / (GetTime(i + 1) - GetTime(i));
      }
      return deltaPerTimeSum / (Count - 1);
    }

  }

  /// <summary>
  /// A ring buffer of floats with a Delta() function that computes the buffer's
  /// average change over time. Delta() will return zero if the buffer contains one
  /// or fewer samples.
  /// 
  /// The larger the buffer, the more stable but also delayed the resulting average
  /// change over time. A buffer size of 5 is a good start for 60-90 Hz updates.
  /// </summary>
  public class DeltaFloatBuffer : DeltaBuffer<float, float> {

    public DeltaFloatBuffer(int bufferSize) : base(bufferSize) { }

    /// <summary>
    /// Returns the average change between each sample per unit time, or zero if the
    /// buffer is empty.
    /// </summary>
    public override float Delta() {
      if (Count <= 1) { return 0f; }

      float deltaPerTimeSum = 0f;
      for (int i = 0; i < Count - 1; i++) {
        deltaPerTimeSum += (Get(i + 1) - Get(i)) / (GetTime(i + 1) - GetTime(i));
      }
      return deltaPerTimeSum / (Count - 1);
    }

  }

  /// <summary>
  /// A ring buffer of Quaternions with a Delta() function that computes the buffer's
  /// average change over time as an angle-axis vector. Returns Vector3.zero if the
  /// buffer contains one or fewer samples.
  /// 
  /// The larger the buffer, the more stable but also delayed the resulting average
  /// change over time. A buffer size of 5 is a good start for 60-90 Hz updates.
  /// </summary>
  public class DeltaQuaternionBuffer : DeltaBuffer<Quaternion, Vector3> {

    public DeltaQuaternionBuffer(int bufferSize) : base(bufferSize) { }

    /// <summary>
    /// Returns the average angular velocity of Quaternions in the buffer as an
    /// angle-axis vector, or zero if the buffer is empty.
    /// </summary>
    public override Vector3 Delta() {
      if (Count <= 1) return Vector3.zero;

      var deltaSum = Vector3.zero;
      for (int i = 0; i < Count - 1; i++) {
        var sample0 = _buffer.Get(i);
        var sample1 = _buffer.Get(i + 1);
        var r0 = sample0.value;
        var t0 = sample0.time;
        var r1 = sample1.value;
        var t1 = sample1.time;

        var delta = (r1.From(r0)).ToAngleAxisVector();
        var deltaTime = t1.From(t0);

        deltaSum += delta / deltaTime;
      }

      return deltaSum / (Count - 1);
    }

  }

}