A client implementation of the LeapJS protocol

LeapWebsocketProvider.cs

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using NativeWebSocket;
using Leap.Unity.Encoding;

namespace Leap.Unity {
  public class LeapWebsocketProvider : LeapProvider {
    WebSocket _ws;
    Frame _untransformedUpdateFrame = new Frame(), _transformedUpdateFrame = new Frame();
    VectorHand _leftVHand = new VectorHand(), _rightVHand = new VectorHand();
    Hand       _leftHand  = new Hand      (), _rightHand  = new Hand(); List<Hand> _hands = new List<Hand>();
    JSONFrame _currentJSONFrame;

    void Start() {
      // Create WebSocket instance
      _ws = new WebSocket("ws://127.0.0.1:6437/v6.json"); // May have to switch to wss:6436 for https pages

      // Parse Incoming Frames
      _ws.OnMessage += (byte[] msg) => {
        string frameUpdate = System.Text.Encoding.UTF8.GetString(msg);
        JSONFrame tempFrame = JsonUtility.FromJson<JSONFrame>(frameUpdate);
        if (tempFrame.timestamp > 0) {
          _currentJSONFrame = tempFrame;
        } else {
          Debug.Log("Service Info: " + frameUpdate);
        }
      };

      // Handle other Websocket Lifetime Events
      _ws.OnOpen  += ()                        => {
        Debug.Log("WS connected with state: " + _ws.State.ToString());
        _ws.Send(System.Text.Encoding.UTF8.GetBytes("{\"optimizeHMD\":true}"));
      };
      _ws.OnError += (string errMsg)           => { Debug.Log("WS error: "            + errMsg); };
      _ws.OnClose += (WebSocketCloseCode code) => { Debug.Log("WS closed with code: " + code.ToString()); };

      _ws.Connect();
    }
    private void OnApplicationQuit() { _ws.Close(); }

    // Dispatch the frames we've received to the far corners of the scene
    // TODO: Consider reinterpolating the incoming frames here
    void Update     () {
      //Debug.Log("Have we received a frame yet? : " + hasReceivedFrameYet);
      #if !UNITY_WEBGL || UNITY_EDITOR
      _ws.DispatchMessageQueue();
      #endif
      fillLeapFrameFromJSONFrame(_currentJSONFrame, ref _untransformedUpdateFrame);
      DispatchUpdateFrameEvent(_untransformedUpdateFrame); 
    }
    void FixedUpdate() {
      //fillLeapFrameFromJSONFrame(_currentJSONFrame, ref _untransformedUpdateFrame);
      DispatchFixedFrameEvent(_untransformedUpdateFrame); 
    }

    #region JSON Schema Implementation
    [Serializable]
    public struct JSONHand {
      public int id;
      public string type;
      public float armWidth, confidence, grabAngle, grabStrength, palmWidth, pinchDistance, pinchStrength, timeVisible;
      public float[] direction, elbow, palmNormal, palmPosition, palmVelocity, wrist;
      public float[][] armBasis;
    }
    [Serializable]
    public struct JSONPointable {
      public int id, handId;
      public int type;
      public float timeVisible, length, width;
      public float[] btipPosition, carpPosition, dipPosition, direction, mcpPosition, pipPosition, tipPosition;
      public float[][] bases;
    }
    [Serializable]
    public struct JSONFrame {
      public long id;
      public long timestamp;
      public float currentFrameRate;
      public JSONHand[] hands;
      public JSONPointable[] pointables;
    }

    public void fillLeapFrameFromJSONFrame(JSONFrame jsonFrame, ref Frame leapFrame) {
      leapFrame.Id = jsonFrame.id;
      leapFrame.Timestamp = jsonFrame.timestamp;
      leapFrame.CurrentFramesPerSecond = jsonFrame.currentFrameRate;

      _hands.Clear(); JSONHand? workingHandLeft, workingHandRight;

      // Fill joint array from JSON Hand and Pointables and add to Frame
      if ((workingHandLeft = GetJSONHand(jsonFrame, true)).HasValue) {
        FillJointsFromJSON(workingHandLeft.Value, jsonFrame.pointables, ref _leftVHand);
        _leftVHand.Decode(_leftHand);
        SetHandMetadata(jsonFrame, workingHandLeft.Value, _leftHand);
        _hands.Add(_leftHand);
      }
      if ((workingHandRight = GetJSONHand(jsonFrame, false)).HasValue) {
        FillJointsFromJSON(workingHandRight.Value, jsonFrame.pointables, ref _rightVHand);
        _rightVHand.Decode(_rightHand);
        SetHandMetadata(jsonFrame, workingHandRight.Value, _rightHand);
        _hands.Add(_rightHand);
      }
      leapFrame.Hands = _hands;
    }
    #endregion

    #region Editor Pose Implementation

    #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 {
        if (_cachedLeftHands.TryGetValue(editTimePose, out Hand 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 {
        if (_cachedRightHands.TryGetValue(editTimePose, out Hand 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
        return _untransformedUpdateFrame;
      }
    }

    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
        return _untransformedUpdateFrame;
      }
    }
    #endregion

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

    protected JSONHand? GetJSONHand(JSONFrame frame, bool isLeft) {
      if (frame.hands == null) return null;
      foreach (JSONHand hand in frame.hands) {
        if (hand.type.Equals("left") && isLeft) {
          return hand;
        } else if (!hand.type.Equals("left") && !isLeft) {
          return hand;
        }
      }
      return null;
    }

    protected JSONPointable GetJSONFinger(JSONHand hand, JSONPointable[] pointables, int index) {
      foreach (JSONPointable pointable in pointables) {
        if (pointable.handId == hand.id && pointable.type == index) {
          return pointable;
        }
      }
      Debug.LogError("Something is wrong!  Finger doesn't exist?!  Hand is: "+ JsonUtility.ToJson(hand) + ", and index is: " + index, this);
      return new JSONPointable();
    }

    protected Vector3 JSToUnityVector(float[] JSVector) {
      return new Vector3(JSVector[0] / 1000.0f, JSVector[1] / 1000.0f, -JSVector[2] / 1000.0f);
    }

    protected void FillJointsFromJSON(JSONHand hand, JSONPointable[] pointables, ref VectorHand vHand) {
      // 1. Set the rigid pose of the VectorHand
      vHand.palmPos   = JSToUnityVector(hand.palmPosition);
      Vector3 down    = JSToUnityVector(hand.palmNormal);
      Vector3 forward = JSToUnityVector(hand.direction);
      vHand.palmRot   = Quaternion.LookRotation(forward, -down);
      vHand.isLeft    = hand.type.Equals("left");

      // 2. Construct the array of 25 joints in that local space and set them inside the VectorHand
      for (int f = 0; f < 5; f++) {
        JSONPointable finger = GetJSONFinger(hand, pointables, f);
        float[][] jointPositions = new float[][] { finger.carpPosition, finger.mcpPosition, finger.pipPosition, finger.dipPosition, finger.btipPosition };

        for (int j = 0; j < 5; j++) {
          Vector3 joint = JSToUnityVector(jointPositions[j]);
          Vector3 localJoint = VectorHand.ToLocal(joint, vHand.palmPos, vHand.palmRot);
          vHand.jointPositions[(f * 5) + j] = localJoint;
        }
      }

      // 3. Move Hands from Device Space to World Space
      vHand.palmPos = (Quaternion.Euler(-90f, 180f, 0f) * vHand.palmPos) + (Vector3.forward * 0.1f);
      vHand.palmRot =  Quaternion.Euler(-90f, 180f, 0f) * vHand.palmRot;
      vHand.palmPos = transform.TransformPoint(vHand.palmPos);
      vHand.palmRot = transform.rotation * vHand.palmRot;
    }

    protected void SetHandMetadata(JSONFrame jsonFrame, JSONHand jsonHand, Hand leapHand) {
      leapHand.PinchDistance = jsonHand.pinchDistance;
      leapHand.PinchStrength = jsonHand.pinchStrength;
      Quaternion localToWorld = transform.rotation * Quaternion.Euler(-90f, 180f, 0f);
      Vector3 vel            = localToWorld * JSToUnityVector(jsonHand.palmVelocity);
      leapHand.PalmVelocity  = new Vector(vel.x, vel.y, vel.z);
      leapHand.FrameId       = jsonFrame.id;
      leapHand.GrabStrength  = jsonHand.grabStrength;
      leapHand.GrabAngle     = jsonHand.grabAngle;
      leapHand.PalmWidth     = jsonHand.palmWidth;
      leapHand.IsLeft        = jsonHand.type.Equals("left");

      for(int i = 0; i < 5; i++) {
        JSONPointable jsonFinger = GetJSONFinger(jsonHand, jsonFrame.pointables, i);
        Vector3 fDir = localToWorld * JSToUnityVector(jsonFinger.direction);
        leapHand.Fingers[i].Direction = new Vector(fDir.x, fDir.y, fDir.z);

        leapHand.Fingers[i].Width  = jsonFinger.width;
        leapHand.Fingers[i].HandId = jsonHand  .id;
        leapHand.Fingers[i].Id     = jsonFinger.id;
        leapHand.Fingers[i].Length = jsonFinger.length;
      }
    }
    #endregion
  }
}