thesis-motion-guidance/Assets/KinectView/Scripts/BodySourceView.cs

using System.Collections.Generic;
using UnityEngine;
using Kinect = Windows.Kinect;
using Valve.VR;

public class BodySourceView : MonoBehaviour
{
    public Transform[] joints;
    public GameObject BodySourceManager;
    public Transform cameraRig;
    public PlayerReplay playerReplay;
    public ModeController modeController;

    // TODO: Implementing color feedback
    public GameObject body;

    public bool wristLeftLate;
    public bool wristRightLate;

    private Dictionary<ulong, GameObject> _Bodies = new Dictionary<ulong, GameObject>();
    private BodySourceManager _BodyManager;

    private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>()
    {
        { Kinect.JointType.FootLeft, Kinect.JointType.AnkleLeft },
        { Kinect.JointType.AnkleLeft, Kinect.JointType.KneeLeft },
        { Kinect.JointType.KneeLeft, Kinect.JointType.HipLeft },
        { Kinect.JointType.HipLeft, Kinect.JointType.SpineBase },

        { Kinect.JointType.FootRight, Kinect.JointType.AnkleRight },
        { Kinect.JointType.AnkleRight, Kinect.JointType.KneeRight },
        { Kinect.JointType.KneeRight, Kinect.JointType.HipRight },
        { Kinect.JointType.HipRight, Kinect.JointType.SpineBase },

        { Kinect.JointType.HandTipLeft, Kinect.JointType.HandLeft },
        { Kinect.JointType.ThumbLeft, Kinect.JointType.HandLeft },
        { Kinect.JointType.HandLeft, Kinect.JointType.WristLeft },
        { Kinect.JointType.WristLeft, Kinect.JointType.ElbowLeft },
        { Kinect.JointType.ElbowLeft, Kinect.JointType.ShoulderLeft },
        { Kinect.JointType.ShoulderLeft, Kinect.JointType.SpineShoulder },

        { Kinect.JointType.HandTipRight, Kinect.JointType.HandRight },
        { Kinect.JointType.ThumbRight, Kinect.JointType.HandRight },
        { Kinect.JointType.HandRight, Kinect.JointType.WristRight },
        { Kinect.JointType.WristRight, Kinect.JointType.ElbowRight },
        { Kinect.JointType.ElbowRight, Kinect.JointType.ShoulderRight },
        { Kinect.JointType.ShoulderRight, Kinect.JointType.SpineShoulder },

        { Kinect.JointType.SpineBase, Kinect.JointType.SpineMid },
        { Kinect.JointType.SpineMid, Kinect.JointType.SpineShoulder },
        { Kinect.JointType.SpineShoulder, Kinect.JointType.Neck },
        { Kinect.JointType.Neck, Kinect.JointType.Head },
    };

    void Update()
    {
        if (BodySourceManager == null)
        {
            return;
        }

        _BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
        if (_BodyManager == null)
        {
            return;
        }

        Kinect.Body[] data = _BodyManager.GetData();
        if (data == null)
        {
            return;
        }

        List<ulong> trackedIds = new List<ulong>();
        foreach (var body in data)
        {
            if (body == null)
            {
                continue;
            }

            if (body.IsTracked)
            {
                trackedIds.Add(body.TrackingId);
            }
        }

        List<ulong> knownIds = new List<ulong>(_Bodies.Keys);

        // First delete untracked bodies
        foreach (ulong trackingId in knownIds)
        {
            if (!trackedIds.Contains(trackingId))
            {
                Destroy(_Bodies[trackingId]);
                _Bodies.Remove(trackingId);
            }
        }

        foreach (var body in data)
        {
            if (body == null)
            {
                continue;
            }

            if (body.IsTracked)
            {
                if (!_Bodies.ContainsKey(body.TrackingId))
                {
                    _Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
                }

                RefreshBodyObject(body, _Bodies[body.TrackingId]);
            }
        }
    }

    private GameObject CreateBodyObject(ulong id)
    {
        body = new GameObject("Body:" + id);
        body.transform.parent = gameObject.transform;

        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            GameObject jointObj = GameObject.CreatePrimitive(PrimitiveType.Cube);

            // Give WristLeft and WristRight rigidbody
            if (jt == Kinect.JointType.WristLeft)
            {
                jointObj.tag = "WristLeft";
                Rigidbody rb = jointObj.AddComponent<Rigidbody>();
                rb.useGravity = false;
                rb.isKinematic = true;
            }
            else if (jt == Kinect.JointType.WristRight)
            {
                jointObj.tag = "WristRight";
                Rigidbody rb = jointObj.AddComponent<Rigidbody>();
                rb.useGravity = false;
                rb.isKinematic = true;
            }

            LineRenderer lr = jointObj.AddComponent<LineRenderer>();
            //lr.SetVertexCount(2);
            lr.positionCount = 2;
            //lr.SetWidth(0.05f, 0.05f);
            lr.startWidth = 0.05f;
            lr.endWidth = 0.05f;
            Material whiteDiffuseMat = new Material(Shader.Find("Sprites/Default"));
            whiteDiffuseMat.color = new Color(1, 1, 1, 0.5f);
            lr.material = whiteDiffuseMat;

            jointObj.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
            jointObj.name = jt.ToString();
            jointObj.transform.parent = body.transform;
        }

        return body;
    }

    private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
    {
        Vector3[] initJoints = new Vector3[25];
        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];

            initJoints[(int)jt] = GetVector3FromJoint(sourceJoint);
        }

        Vector3[] rescaledJoints = RescaleJoints(initJoints);

        for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
        {
            Kinect.Joint sourceJoint = body.Joints[jt];
            Kinect.Joint? targetJoint = null;

            if (_BoneMap.ContainsKey(jt))
            {
                targetJoint = body.Joints[_BoneMap[jt]];
            }

            Transform jointObj = bodyObject.transform.Find(jt.ToString());
            //jointObj.localPosition = GetVector3FromJoint(sourceJoint);
            jointObj.localPosition = rescaledJoints[(int)jt];

            if (jt == Kinect.JointType.Head)
            {
                // Make head joint invisible
                jointObj.GetComponent<Renderer>().enabled = false;
            }

            if (jt == Kinect.JointType.Neck)
            {
                // Make camera follow the neck
                if (modeController.perspective == ModeController.Perspective.FirstPersonPerspective)
                {
                    cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y, jointObj.localPosition.z - 0.03f);
                    jointObj.GetComponent<Renderer>().enabled = false;
                }
                else
                {
                    cameraRig.localPosition = new Vector3(jointObj.localPosition.x, jointObj.localPosition.y + 0.1f, jointObj.localPosition.z + 0.5f);
                }
            }

            // Make these joints invisible
            if (jt == Kinect.JointType.ThumbLeft || jt == Kinect.JointType.ThumbRight
                    || jt == Kinect.JointType.HandLeft || jt == Kinect.JointType.HandRight
                    || jt == Kinect.JointType.HandTipLeft || jt == Kinect.JointType.HandTipRight)
            {
                jointObj.GetComponent<Renderer>().enabled = false;
            }

            // WristLeft and WristRight do something if not following the motions
            //if ((jt == Kinect.JointType.WristLeft && wristLeftLate) || (jt == Kinect.JointType.WristRight && wristRightLate))
            //{
            //    if (modeController.feedback == ModeController.Feedback.ColorFeedback)
            //    {
            //        jointObj.GetComponent<Renderer>().material.color = new Color(1, 0, 0);
            //        jointObj.GetComponent<LineRenderer>().startColor = new Color(1, 0, 0);
            //    }
            //    else if (modeController.feedback == ModeController.Feedback.HapticFeedback)
            //    {
            //        // TODO: Add haptic feedback need to test
            //        if (jt == Kinect.JointType.WristLeft)
            //        {
            //            Debug.Log("WristLeft vibrate");
            //            SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.LeftHand].Execute(0, 0.01f, 10, 1);
            //        }
            //        else if (jt == Kinect.JointType.WristRight)
            //        {
            //            Debug.Log("WristRight vibrate");
            //            SteamVR_Actions.default_Haptic[SteamVR_Input_Sources.RightHand].Execute(0, 0.01f, 10, 1);
            //        }
            //    }
            //}

            LineRenderer lr = jointObj.GetComponent<LineRenderer>();
            if (targetJoint.HasValue && jt != Kinect.JointType.Neck && jt != Kinect.JointType.ThumbLeft && jt != Kinect.JointType.ThumbRight
                    && jt != Kinect.JointType.HandLeft && jt != Kinect.JointType.HandRight
                    && jt != Kinect.JointType.HandTipLeft && jt != Kinect.JointType.HandTipRight) // Do not make LineRenderer for these joints
            {
                lr.SetPosition(0, jointObj.localPosition);
                //lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
                lr.SetPosition(1, rescaledJoints[(int)_BoneMap[jt]]);

                // Coloring the line renderer
                //lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
                //lr.startColor = GetColorForState(sourceJoint.TrackingState);
                //lr.endColor = GetColorForState(targetJoint.Value.TrackingState);
            }
            else
            {
                lr.enabled = false;
            }

            // Record joints to PlayerReplay.cs
            playerReplay.joints[(int)jt] = jointObj;
        }

        // Joint orientations
        //Kinect.Vector4[] orientation = new Kinect.Vector4[22];
        //orientation[0] = body.JointOrientations[Kinect.JointType.Head].Orientation;
        //orientation[1] = body.JointOrientations[Kinect.JointType.Neck].Orientation;
        //orientation[2] = body.JointOrientations[Kinect.JointType.SpineMid].Orientation;
        //orientation[3] = body.JointOrientations[Kinect.JointType.ShoulderLeft].Orientation;
        //orientation[4] = body.JointOrientations[Kinect.JointType.ShoulderRight].Orientation;
        //orientation[5] = body.JointOrientations[Kinect.JointType.ElbowLeft].Orientation;
        //orientation[6] = body.JointOrientations[Kinect.JointType.ElbowRight].Orientation;
        //orientation[7] = body.JointOrientations[Kinect.JointType.WristLeft].Orientation;
        //orientation[8] = body.JointOrientations[Kinect.JointType.WristRight].Orientation;
        //orientation[9] = body.JointOrientations[Kinect.JointType.HipLeft].Orientation;
        //orientation[10] = body.JointOrientations[Kinect.JointType.HipRight].Orientation;
        //orientation[11] = body.JointOrientations[Kinect.JointType.KneeLeft].Orientation;
        //orientation[12] = body.JointOrientations[Kinect.JointType.KneeRight].Orientation;
        //orientation[13] = body.JointOrientations[Kinect.JointType.SpineBase].Orientation;
        //orientation[14] = body.JointOrientations[Kinect.JointType.AnkleLeft].Orientation;
        //orientation[15] = body.JointOrientations[Kinect.JointType.AnkleRight].Orientation;
        //orientation[16] = body.JointOrientations[Kinect.JointType.FootLeft].Orientation;
        //orientation[17] = body.JointOrientations[Kinect.JointType.FootRight].Orientation;
        //orientation[18] = body.JointOrientations[Kinect.JointType.HandLeft].Orientation;
        //orientation[19] = body.JointOrientations[Kinect.JointType.HandRight].Orientation;
        //orientation[20] = body.JointOrientations[Kinect.JointType.ThumbLeft].Orientation;
        //orientation[21] = body.JointOrientations[Kinect.JointType.ThumbRight].Orientation;

        //for (int j = 0; j < 22; j++)
        //{
        //    //joints[j].rotation = ConvertKinectOrientationToUnity(orientation[j]);
        //}
    }

    private static Color GetColorForState(Kinect.TrackingState state)
    {
        switch (state)
        {
            case Kinect.TrackingState.Tracked:
                return Color.green;

            case Kinect.TrackingState.Inferred:
                return Color.red;

            default:
                return Color.black;
        }
    }

    private Vector3 GetVector3FromJoint(Kinect.Joint joint)
    {
        //return new Vector3(joint.Position.X * -10, joint.Position.Y * 10, joint.Position.Z * 10);

        return new Vector3(joint.Position.X * -1, joint.Position.Y, joint.Position.Z);
    }

    private Quaternion ConvertKinectOrientationToUnity(Kinect.Vector4 orientation)
    {
        Quaternion orientationUnity;
        orientationUnity.x = -orientation.X;
        orientationUnity.y = orientation.Y;
        orientationUnity.z = orientation.Z;
        orientationUnity.w = orientation.W;
        return orientationUnity;
    }

    private Vector3[] RescaleJoints(Vector3[] initJoints)
    {
        Vector3[] rescaledJoints = new Vector3[25];

        rescaledJoints[(int)Kinect.JointType.SpineBase] = Vector3.zero;

        // Lower part
        rescaledJoints[(int)Kinect.JointType.HipLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipLeft], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];
        rescaledJoints[(int)Kinect.JointType.HipRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.HipRight], 0.08f) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.KneeLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipLeft], initJoints[(int)Kinect.JointType.KneeLeft], 0.25f) + rescaledJoints[(int)Kinect.JointType.HipLeft];
        rescaledJoints[(int)Kinect.JointType.KneeRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.HipRight], initJoints[(int)Kinect.JointType.KneeRight], 0.25f) + rescaledJoints[(int)Kinect.JointType.HipRight];

        rescaledJoints[(int)Kinect.JointType.AnkleLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeLeft], initJoints[(int)Kinect.JointType.AnkleLeft], 0.3f) + rescaledJoints[(int)Kinect.JointType.KneeLeft];
        rescaledJoints[(int)Kinect.JointType.AnkleRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.KneeRight], initJoints[(int)Kinect.JointType.AnkleRight], 0.3f) + rescaledJoints[(int)Kinect.JointType.KneeRight];

        rescaledJoints[(int)Kinect.JointType.FootLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleLeft], initJoints[(int)Kinect.JointType.FootLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleLeft];
        rescaledJoints[(int)Kinect.JointType.FootRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.AnkleRight], initJoints[(int)Kinect.JointType.FootRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.AnkleRight];

        // Upper part
        rescaledJoints[(int)Kinect.JointType.SpineMid] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineBase], initJoints[(int)Kinect.JointType.SpineMid], 0.2f) + rescaledJoints[(int)Kinect.JointType.SpineBase];

        rescaledJoints[(int)Kinect.JointType.SpineShoulder] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineMid], initJoints[(int)Kinect.JointType.SpineShoulder], 0.2f) + rescaledJoints[(int)Kinect.JointType.SpineMid];

        rescaledJoints[(int)Kinect.JointType.ShoulderLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderLeft], 0.1f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];
        rescaledJoints[(int)Kinect.JointType.ShoulderRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.ShoulderRight], 0.1f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        rescaledJoints[(int)Kinect.JointType.ElbowLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderLeft], initJoints[(int)Kinect.JointType.ElbowLeft], 0.2f) + rescaledJoints[(int)Kinect.JointType.ShoulderLeft];
        rescaledJoints[(int)Kinect.JointType.ElbowRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ShoulderRight], initJoints[(int)Kinect.JointType.ElbowRight], 0.2f) + rescaledJoints[(int)Kinect.JointType.ShoulderRight];

        rescaledJoints[(int)Kinect.JointType.WristLeft] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowLeft], initJoints[(int)Kinect.JointType.WristLeft], 0.2f) + rescaledJoints[(int)Kinect.JointType.ElbowLeft];
        rescaledJoints[(int)Kinect.JointType.WristRight] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.ElbowRight], initJoints[(int)Kinect.JointType.WristRight], 0.2f) + rescaledJoints[(int)Kinect.JointType.ElbowRight];

        rescaledJoints[(int)Kinect.JointType.Neck] = GetTargetJointWithDesiredLength(
            initJoints[(int)Kinect.JointType.SpineShoulder], initJoints[(int)Kinect.JointType.Neck], 0.07f) + rescaledJoints[(int)Kinect.JointType.SpineShoulder];

        return rescaledJoints;
    }

    private Vector3 GetTargetJointWithDesiredLength(Vector3 source, Vector3 target, float desiredLength)
    {
        Vector3 sourceToTarget = target - source;
        float length = sourceToTarget.magnitude;
        return sourceToTarget * desiredLength / length;
    }
}